SCREW THEORY BASED PATH TRACKING TECHNIQUES FOR
AUTONOMOUS ROBOTS AND MANIPULATORS
By
WAHEED A. ABBASI
A DISSERTATION PRESENTED TO THE GRADUATE SCHOOL
OF THE UNIVERSITY OF FLORIDA IN PARTIAL FULFILLMENT
OF THE REQUIREMENTS FOR THE DEGREE OF
DOCTOR OF PHILOSOPHY
UNIVERSITY OF FLORIDA
2001
Copyright 2001
by
Waheed A. Abbasi
ACKNOWLEDGMENTS
I would like to express my thanks and appreciation to the members of my
supervisory committee: Dr. Carl Crane, Dr. Joseph Duffy, Dr. John Ziegert, Dr. John
Schueller, and Dr. Michael Nechyba for their support and guidance. I extend special
thanks to committee chairman Dr. Crane, for his guidance, patronage, and support
throughout my tenure.
I thank Dr. Jose Rico Martinez for his invaluable input and his friendship. I also
thank my friends and colleagues in CIMAR who provided a very cordial and friendly
work enviroimient.
I give my heartfelt gratitude to my parents, whose love, dedication, and
encouragement made whatever I have done possible. Finally, I give special thanks to my
wife, Uzma Abbasi, who provided continuous encouragement and the inspiration needed
to finish this dissertation. " . 
'f^ ^
^■^I».
TABLE OF CONTENTS
page
ACKNOWLEDGMENTS iv
ABSTRACT vii
1 INTRODUCTION 1
Motivation 1
Background 2
Research Objectives 2
2 BACKGROUND ON THEORY OF SCREWS 4
Definitions and Basic Concepts 4
Statics 10
Kinematics 15
3 REVIEW OF LITERATURE 20
Techniques to Improve/Modify the Path Planner 21
Techniques to Improve Path Tracking 29
4 PATH TRACKING METHODOLOGIES 45
Autonomous Robot / Vehicle 46
Considering Pose Information Only 50
Considering Pose and Velocity Information 51
3R Planar Manipulator 61
Considering Pose Information Only 61
Considering Pose and Velocity Information 67
Parallel Manipulators 76
Tracking Methodology 77
66 Parallel Manipulator 79
Algorithm Implementation 81
5 PATH TRACKING AND CONTROL 91
Manipulator Model 91
Motor Specifications 94
System Model 95
Controller Development 97
Controller Implementation 100
6 CONCLUSIONS AND FUTURE WORK 108
Conclusions 108
Future Work 109
APPENDIX
A INITIAL PATHTRACKING APPROACHES Ill
B SIMULATION SOURCE CODE 116
LIST OF REFERENCES 155
BIOGRAPHICAL SKETCH 159
'r t ■ y •' ' ■ 
VI
Abstract of Dissertation Presented to the Graduate School
of the University of Florida in Partial Fulfillment of the
Requirements for the Degree of Doctor of Philosophy
SCREW THEORY BASED PATH TRACKING TECHNIQUES FOR
AUTONOMOUS ROBOTS AND MANIPULATORS
By
WAHEED A. ABBASI
August 2001
Chairman: Dr. Carl D. Crane III
Major Department: Mechanical Engineering
The Theory of Screws, invented by Sir Robert Ball, has been used in the
mechanisms area for over three decades. Engineers and scientists at the Center for
Intelligent Machines & Robotics (CIMAR) have successfiilly used the Theory for a
variety of applications including analysis of serial and parallel mechanisms, tensegrity
structures, and vibration analysis. The theory provides for an elegant and geometrically
meaningful way for conducting such analyses.
Following a planned path is an essential element in most applications of robots
and manipulators. Following a path necessitates the determination of how accurately the
path is followed. This is generally known as path tracking. This research is aimed at
applying the Theory of Screws to the path tracking problem. The research forays into
developing a path tracking methodology for autonomous vehicles, serial robots, and
parallel manipulators based on Screw Theory.
vu
The path tracking problem is addressed by developing mathematical basis for
generating the tracking commands. The techniques are based on whether the tracking
needs to be done at the position level or at the velocity level, i.e., whether the trajectory
has to be continuous at the position level or at the velocity level. The basic models are
developed including the resultant of combining a velocity and an acceleration command
screw. The developed path tracking methodology is applied to autonomous robots and
serial manipulators. A separate methodology is developed for parallel mechanisms. A
technique is devised that satisfies the motion criteria for the desired position, velocity,
and acceleration commands. The technique provides the connector displacements,
velocities, accelerations, and forces for a prescribed motion of the platform. The
methodology can be used as a mechanism performance evaluation tool. Simulations are
performed to validate the results.
A dynamic model of a 3R manipulator is developed to test the path tracking
algorithm with a control system. A PID controller is used for manipulator motor control.
The control system is modeled with a time delay, which is varied to study its effects on
system performance. The results show that the path tracking techniques function for the
test manipulator within a range of time delays.
vm
CHAPTER 1
INTRODUCTION
Motivation
The motivation for this work comes from the elegance with which the Theory of
Screws has been used for the kinematic and dynamic analyses of robot manipulators.
The initiative is to employ the Theory of Screws (Screw Theory) and attempt to develop
a methodology for path tracking of robots.
Following a planned path is a fundamental function in most applications of
robots, manipulators, machine tools, and autonomous vehicles V In most cases, following
a planned path is not only necessary but is critical in accomplishing the task;
necessitating the determination of how accurately the path is followed. This is
accomplished by what is known as path tracking.
Path tracking in general can be defined as following a prescribed path with
possible additional constraints. These additional constraints are generally system or task
dependent. For robot manipulators and autonomous vehicles, these constraints can be
orientation, time, velocity, and acceleration. A cursory review of scientific literature in
the area of path tracking shows that path tracking is in general either a subsystem in the
overall control scheme of the robot or is embedded in the control system.
The term robot is used as a generality; it encompasses manipulators (serial and parallel),
machine tools, and autonomous vehicles.
Background
Work in the area of mechanisms (among other research areas) has been ongoing
at the Center for Intelligent Machines and Robotics (CIMAR) for decades. Research has
been conducted and published on mechanism analysis, ranging from direct (forward) and
inverse (reverse) analysis of mechanisms to velocity, acceleration, and jerk analysis of
mechanisms.
An area of particular thrust has been the design and development of parallel
mechanisms for a variety of applications. Parallel platforms have been, and are being,
investigated for applications ranging from passive feedback devices to milling machines.
The development of mechanical robotic systems requires the need for a control
system to accomplish the prescribed task that it is designed for. The idea of using Screw
Theory to accomplish this task arose from this need.
• Research Objectives
The objective of this research is to investigate the use of Screw Theory as a tool
for path tracking of robot manipulators. The intent is to develop a methodology that lays
the framework for tracking control. Techniques will be developed for tracking control of
planar serial manipulators (3R), parallel manipulators (66 spatial manipulator), and
autonomous vehicles. These techniques are implemented in simulation and results
presented to show the performance and viability of the techniques.
The material is arranged in the following order. Chapter 2 offers a brief
introduction to the Theory of Screws and to the concepts that are used in this research.
Chapter 3 reviews the current state of literature as it pertains to the control of robotic
manipulators in general, and to path tracking in particular. Chapter 4 presents the
approaches that have been developed and the simulations that have been done to illustrate
and validate the methodologies. Chapter 5 presents an application of the path tracking
methodology to a 3R Manipulator with a system model and application of control
strategies. Chapter 6 summarizes the pertinent work that has been done, concludes the
results, and provides an outline for future work.
*, .^
CHAPTER 2
BACKGROUND ON THEORY OF SCREWS
Since the work presented in this manuscript derives from the Theory of Screws, a
brief overview of the relevant topics in the theory are presented to facihtate
understanding. This introduction is not intended to be comprehensive, just illustrative
enough to familiarize the reader with the material that follows. After the introductory
material, results pertaining to the velocity and acceleration analyses of mechanisms based
on Duffy, Crane, and Rico's work are presented.
Screw theory has been used in kinematics for more than two centuries. Sir Robert
Ball conducted the principal and the most comprehensive fundamental work in this area
at the end of the 19th century. This work was published as A Treatise on the Theory of
Screws in 1900 [BalOO]. Except for a few isolated contributions in the first decades of
this century, screw theory lay dormant until it was rediscovered in the 1960s by Hunt and
Phillips [Hun78, Phi84] who made very important advances in the field. Since then, the
theory has been used extensively in kinematics: fiom the search of overconstrained
linkages, grasping analysis, velocity and singularity analysis of serial manipulators, to
force, velocity and acceleration analysis of parallel manipulators [Mar96].
Definitions and Basic Concepts
Ball defined a screw in a manner that was impervious to whether its usage was in
kinematics or dynamics. A screw is a geometrical identity described as "a straight line
with which a definite linear magnitude termed the pitch is associated." In defining the
pitch, Ball considers a rigid body that needs to go from one position to a desired position.
A certain axis can be found such that if the body is rotated around that axis for a
determinate angle and translated parallel to the axis for a determinate distance, the
desired movement can be achieved. The situation is analogous to a body attached to the
nut of a uniform screw (in the ordinary sense of the word) which has an appropriate
position in space, and an appropriate number of threads to the inch. Thus the pitch of a
screw is defined to be the rectilinear distance through which the nut is translated parallel
to the axis of the screw, while the nut is rotated through the angular unit of circular
measure. The pitch is thus a linear magnitude. It follows from this definition that the
rectilinear distance parallel to the axis of the screw through which the nut moves when
rotated through a given angle is simply the product of the pitch of the screw and the
circular measure of the angle.
Twist: A body is said to receive a twist about a screw when it is rotated
uniformly about the screw, while it is translated uniformly parallel to the screw, through a
distance equal to the product of the pitch and the circular measure of the angle of
rotation. Whatever the movement of a rigid body, it is at every instant twisting about a
screw. For the movement of the body when passing from one position to another
adjacent position, is indistinguishable from the twist about an appropriately chosen screw
by which the same displacement could be affected. The screw about which the body is
twisting at any instant is termed the instantaneous screw .
A system of forces acting on a rigid body may be generally expressed by a certain
force and a couple whose plane is perpendicular to the force [Poi06]. A wrench is
defined as a force and a couple in a plane perpendicular to the force. A wrench is thus a
force directed along the screw and a couple in a plane perpendicular to the screw, the
moment of the couple being equal to the product of the force and the pitch of the screw.
It follows from this that if the pitch is zero, the wrench reduces to a pure force along the
axis of the screw. The Lie product is a product of two screws It is also known as the
Motor Product or Dual Vector Product and is defined as
[$, $2] = [(<o,;Do,) (o>2;t)o2)]
= (o), XCO2 ;(i),xDq2(02XDq, ) C2.1)
A more mathematical description of entities follows.
Consider two distinct points ri(xi,yi zi) and r2(x2,y2,Z2) in space. The join of
these two distinct points determines a line (Figure 2.1). The vector S directed along the
line can be written as
S = (r2r,) (2.2)
which may be expressed as
S^Li+Mj + Nk (2.3)
where L = X2x^ ,M = y2y^ ,N = 2.^2^ are defined as the direction ratios.
From Equation (2.3), the direction ratios are related to the distance \S\ by
»
'■:*
Z'+M'+iV' = S . (2.4)
/ .
Often the entities L, M, and N are expressed as
s s s ^^^^
which represent the unit direction ratios or the direction cosines of the line. In this case
(2.4) reduces to
L'+M^+N^ = 1.
(2.6)
Figure 2. 1 : Two points determine a line.
In Figure 2. 1 , let r be a vector from the origin to any point on the line. The vector
(rri) is parallel to S, thus ' ' ' ,
t •> t
(rr,)xS = 0,
This equation can be written as
where
rxS = So
So = r, X S
(2.7)
(2.8)
(2.9)
represents the moment of the line about the origin O and is origin dependent. Also, since
the vectors S and So are perpendicular they satisfy the orthogonality condition
0.
(2.10)
A way of writing the coordinates of a line is in the form {S;So} and they are
referred to as the Pliicker coordinates of a line. The semicolon is used to signify that the
dimensions of S and So are different. The coordinates {S;So} are homogeneous since
from (2.8) the coordinates {A,S;A,So}, where X is a nonzero scalar, determine the same
line.
Expanding Equation (2.9) yields,
•
1
J
k
x^
yx
^1
L
M
N
(2.11)
which can be written as
where
So = P\+Q\ + R\!i.
P = y,Nz,M .,
Q = z^Lx^N ,
(2.12)
(2.13)
From (2.3) and (2.12) the orthogonality condition S.So=0 can be expressed in the form
LP + MQ + NR ^ (2.14)
If homogeneous coordinates of two points in space are provided as (l;xi,yi,zi)
and (l;x2,y2,Z2), the Pliicker coordinates of the line (Figure 2.2) can be expressed by using
the Grassmann method by taking six 2x2 determinants of the array
1 X, y^
1
1 j'\ ^1
1 X., y^ Zj
(2.15)
as
L.
=
1 X,
1 x^
, M =
1 >'■
1 .^2
, iv =
1 ^,
1 z,
P =
y
y
I ^1
2 ^2
, e=
Z, X,
^2 ^2
, R =
X
X2 J
^2
(2.16)
R
M L
Figure 2.2: Plucker coordinates of a line.
Equations (2.2) and (2.9) can be used to determine the Plucker coordinates of a
line when two points on the line are given. It is also important to determine a point on a
line when the PlUcker coordinates are given. If, for example, the coordinates of a line are
given, {L,M,N;P,Q,R}, an infinite number of solutions exist for x,y,and z (corresponding
to the infinite number of points on the line) [Cra99]. For the general case, any arbitrary
value of X may be selected and the corresponding values for y and z may be determined
as:
y =
xMR
z =
xN + Q
(2.17)
L L
The distance of a line from the origin is determined by the length of the vector p,
which originates at O and terminates at a point on the line such that the direction p is
perpendicular to the direction of the line, S. The distance of the line from the origin is
determined as
■'^..^^■i
10
ISol
P = 1^ , (2.18)
When So=0, p=0 and the line passes through the origin and its coordinates are {S;0}.
When S=0, p=Qo, the line is a line at infinity and its coordinates are {0;So}.
A line can be defined by the join of two points as well as by the intersection of
two planes. The array of coordinates {L,M,N;P,Q,R} is known as the ray coordinates for
a line. The line can be considered as a ray of light joining two distinct points on the line.
The array of coordinates {P,Q,R; L,M,N} is known as the axis coordinates for the line
determined by the meet of two planes. In this case, the line can be considered as the axis
of a pencil of planes. Pairs of dual elements, i.e., points or planes can thus form a line. It
is because of this that a line in threedimensional space is considered to be dual with itself
or selfdual. A distinction is made in notation when the line coordinates are specified.
The ray and axis coordinates are represented by s = {S;So} and S = {So;S} respectively.
Statics
Consider a force with magnitude f acting on a rigid body as shown in Figure 2.3.
The force is acting on a line $ with ray coordinates s = {S;So} where S=1. The force f
can be expressed as a scalar multiple fS of the unit vector S which is bound to the line $.
In order to add forces it is necessary to introduce a reference point O. The moment of the
force f about this reference point, i.e., mo can be expressed as mo=rxf where r is a vector
to any point on the line $. This moment may also be expressed as a scalar multiple fSo
where So is the moment vector of the line $, i.e., So^rxS. The action of the force on the
body thus can be expressed as a scalar multiple of the unit line vector, and the
coordinates for the force are given by
11
w = fs = f{S;So} (2.19)
where S • S = 1 and S • Sg = . The above equation can be expressed in the form
w = fs = {f;mo}.
(2.20)
Figure 2.3: Force acting on a rigid body.
It can be seen from Figure 2.3 and mo=rxf that the vector mo changes as the
location of the reference point is changed. When the line $ passes through the reference
point, mo=^0 and the coordinates of the force are {f;0}. Clearly, f is a line bound vector
which is invariant with a change of coordinate systems while mo is origin dependent.
Consider a rigid body on which is acting a force {fi;moi} whose coordinates are
expressed in terms of the reference point O. Equal and opposite coUinear forces whose
coordinates are {fi;mo2} and {fi;mo2} are applied to the body so that the net resultant
force and couple acting on the body remain the same. The forces {fi;moi} and {fi;mo2}
12
may be replaced by a couple which is equal to the sum of the moments of the two line
bound forces about point O, i.e. m=moimo2. The combination of the force {fi;mo2} and
the couple {0;m} has the same effect on the rigid body as the original force {f;moi}.
Now consider an arbitrary system of forces with coordinates {fi;moi}, {f2;mo2}, ...,
{fn;mon} acting on a rigid body. It is assumed at the outset that a reference point O has
been chosen, the magnitudes of the forces fi, f2, ..., fn are specified, and the unit
coordinates of the lines of action of the forces {Si;Soi}, {S2;So2}, ...{Sn;Son} are known.
The forces are translated to point O and moments moi, mo2, ... mon are introduced to yield
an equivalent system of forces and torques that act on the rigid body. The net force
acting on the rigid body is given by
_ f = If. = 4  (221)
and the line of action of this force passes through point O. The moment acting on the
rigid body is given by
m
= Z™o " (2.22)
The net force f (which passes through point O and whose coordinates are written
as {f;0}) and moment mo (which is a nonlinebound vector whose coordinates are
written as {0;mo}) are equivalent to the original set of forces. The coordinates of the
resultant of the force and moment may be written as the sum of {f;0} and {0;mo} as
w = {f;ino} (2.23)
In general f and mo will not be perpendicular and the quantity with coordinates
w={f;mo}, where fmo^O, was defined as a dyname by Plucker. It follows that in general
it is not possible to translate the line of action of f through some point other than point O
13
and have the translated force produce the same net effect on the rigid body as the original
dyname. The moment mo, however, may be resolved into two components ma and mt as
mo = m^ + m,
(2.24)
which are parallel and perpendicular to the force f respectively. The moment ma may be
determined as
m, =(moS)S (2.25)
where S is a unit vector in the direction of the resultant force f. The moment mt is then
determined as
m, = nin — m.
(2.26)
The line of action of force f may now be translated so that the force with
coordinates {f;mi} plus the moment {0;ma} is equivalent to the dyname
{f;0}+{0;mo}(Figure 2.4). The dyname can thus be represented uniquely by a force f
acting on the line of action {S;Sot} where S,,, = — — and a parallel couple ma. This
representation is called a wrench and is attributed to Ball.
f
■ \j . ^,
'/;•■
rxf=m. y
Figure 2.4: A Dyname and a Wrench.
14
The wrench w which is equivalent to the dyname {f;mo} may be written as
w = {f;mo}= {f;m,}+{0;inj (2.27)
It is desirable to express the wrench in terms of f and mo. Since ma is parallel to f
m^ = hf (2.28)
where h is a nonzero scalar which is called the pitch of the wrench. By algebraic
manipulation and using Equation (2.24), the pitch of the wrench is given by
h = pp (2.29)
Clearly the pitch h is an invariant quantity. From (2.26) and (2.27) we have
w = {f;momJ+{0;mJ (2.30)
and substituting ma from (2.28) gives
w = {f;mohf}+{0;mj. (2.31)
The homogeneous coordinates for the line of action of the wrench are {f;mohf} and the
equation for the line is therefore
rxf = {f;mohf}. (2.32)
In the same way as the action of a force upon a body can be expressed as a scalar
multiple of a unit line vector, a wrench acting on a body can be elegantly expressed as a
scalar multiple of a unit screw where
s = {S;So} (2.33)
and S ■ S = 1 . The pitch of the screw is given by
h = SSo.  (2.34)
From (2.33)
s = {S;So} = {S;SohS} + {0;hS}
line couple
The Pliicker coordinates of the screw axis are {S;So hS} and the equation of the axis is
15
rxS = SohS. (2.36)
A screw can therefore be regarded as a line with a pitch.
Kinematics
In this section a brief overview of the resuhs relevant to the approaches in this
research are presented. References are provided appropriately for the reader interested in
details about the derivation and proofs of the results. The results presented here pertain
to the velocity and acceleration analyses of serial manipulators (open chains). The
detailed description of these analyses is presented by Rico and Duffy [Mar96, Ric99].
The velocity analysis of a serial chain consists of two parts. The first part relates
the angular velocity of two arbitrary links in the serial chain, while the second part relates
the velocity of a point fixed in one link as observed from different links (or reference
frames) of the serial chain. The angular velocity of a rigid body m with respect to a body
(or reference frame) j, can be expressed, in terms of auxiliary bodies (or reference
frames), m\, m2, . . . j'+2 aadj+l as follows:
^co" = ^(o^'*' + ^'^'o)^"^ + ... + "^(d"' + "'to'" . (2.37)
where ""'a)" represents the magnitude and the direction of the angular velocity of body n
as measured with respect to body n1. It should be noted that in this proposition, it is not
necessary that the bodies be physically connected at all.
Now consider a point O fixed in body m of a serial chain with respect to the
reference frame j. The velocity of point O in body m as measured with respect to
reference frame _/ is given by
16
'<
y+io;+2
m2c "J"^
mI c^m
j(o.^, ^sr + ;.,cy,,2 ^"sr + ... + „26>._, "^So + „_,«„ 'S: (2.38)
where ^_^(o„ represents the scalar magnitude of the angular velocity of body n as
measured with respect to body n1.
The velocity state of a rigid body m with respect to a reference frame y, is defined
as
^ym _
^(1)'"
'y>:
(2.39)
The velocity state parameters ^co"" and ^Vq are often referred to as the screw
coordinates, $, and are written as
^S" = { ^co'"; X} (2.40)
As an example, consider a twolink manipulator. The velocity state of link 2 with respect
to the base link can be expressed as
0*2 _ Offl , la.2
^$' + '$%
(2.41)
where
0<p2
$^ =
"$' =
'$^
pco^l
["s'l
>o.
= 0fi^2
ho'l
["s'l
= ^CO^
0<jl
^0.
['(O^'
ps^l
>o.
= ,«2
1^2 '
(2.42)
(2.43)
(2.44)
and 'S' are unit vectors.
In XhQ forward velocity analysis, the angular velocity for each revolute and helical
joint and the linear velocity for each prismatic joint are known. The objective is to
17
determine the velocity state of the last link of the manipulator relative to a reference
frame (base). The velocity state can simply be written as (e.g., 6link manipulator)
«(0^
= 0^1
°s'
Oci
+ ,«2
Ic2
lc2
+ ^(O,
2^3
+ 3^4
3c4
3c4
+ a(Os
4c5
4c5
+ 5^6
556
(2.45)
This may be written as
l<r2
2<p3
3a.4
4ff5
506
= o^y, $' + ,6>2'$' + ^0)/$' + jCo/S^ + ^0)^^$' + 5(y/$
(2.46)
where '$^ are the appropriate unitized screw coordinates for each joint axis. This maybe
rewritten in the matrix format
•^0
= nto/S" = J0>
o^'e
(2.47)
where J, called the Jacobian matrix is the 6x6 matrix formed from the unitized screw
coordinates as
J = [<•$' '$^ ^$^ '$" "$' '$^] (2.48)
and (0 is the vector of joint velocities given as
0) = [o«, ,<y2 ^co^ ^(o, ,(o, ^(o^ . (2.49)
From the position analysis all the elements of J are known, coupled with the angular
velocities being known, (2.47) can be solved to determine the velocity state of the end
effector. For the reverse velocity analysis, the Jacobian matrix is again known coupled
with the velocity state of the endeffector relative to the base. The individual joint
velocities can be obtained from
18
CO = J
"(0^
0.,6
Dr,
(2.50)
If the Jacobian matrix is singular, and thus its inverse cannot be obtained, the manipulator
is referred to as being in a singular configuration.
For the acceleration analysis, open serial chain is considered, where body m is an
arbitrary link and the rigid bodyy is the reference frame. Further, it is assumed that
adjacent bodies or links of the serial chain are connected by helical pairs, and O is an
arbitrary point fixed in the reference frame. The reduced acceleration state of an arbitrary
link m, with respect to the reference frame y, is defined by
^a"
/a^^<x^<
(2.51)
and in terms of screws connecting adjacent links, is given as
^A: = ..»..., ^%J'' + ..,,<y.,., J''%^^^ + ... + .„ ,0, . "^S""' + ^_j«^ '"'$'«
+ m2^ml
+ ... , . . .
(2.52)
where [' %^* $ " ] is the Lie product of the screws inside the bracket. This can also be
written as
^X1 = [J]ci, + [L], (2.53)
where J is the Jacobian and L is the summation of all the Lie products.
The Lie product of two screws is also known as the Dual Vector Product and is
defined in the following manner. Consider two screws $i and $2,
19
($, $2) = fe;i^oi) ('y2;t5o2)) (2.54)
then the Lie product is
[$, $2] = fex^2) ; fe X '^02^2X^^01 )]• (2.55)
Rico and Duffy have further extended this analysis to the jerk [Rjc99].
CHAPTER 3
REVIEW OF LITERATURE
Conventionally, path tracking falls under the umbrella of robot control algorithms,
which are typically divided into two stages: path or trajectory planning and path tracking
(path control). Path tracking is generally considered the lower level of control and path
or trajectory planning is considered the higher level. The path tracker attempts to make
the robot's actual position and velocity match some desired values of position and
velocity, which are provided to the controller by the path planner. Although the terms
path and trajectory are used interchangeably, a distinction is sometimes made: the path
is a timeindependent description of the route tracked by the manipulator, and the
trajectory is a timedependent description of the route.
A variety of approaches have been undertaken to improve the path following
performance of robots. These approaches vary from modifying the 'path planner' to
changing the 'path tracker' to an amalgam of both. In some cases, the distinction
between the two is transparent or is lost. Other approaches attempt to optimize a
particular criterion in robot control, for example, tracking time or joint torque. The
approach taken in this research is different because it uses Screw Theory for path
tracking. Literature on this research is lacking. Nonetheless, I present an overview of the
techniques that are most relevant in the following sections. I attempt to group literature
under broad categories although the reader is warned that each approach is relatively
distinct. ;»^ I f i 1* '
20
21
Techniques to Improve/Modify the Path Planner
Robot trajectory refers to a time history of position, velocity, and acceleration for
each degree of freedom. Many robot applications require the endeffector to track a
specified path for the purpose of accomplishing the task satisfactorily and avoiding
obstacles. For achieving this objective, the trajectory planning can be conducted either in
the joint variable space or in the Cartesian space.
The earliest work of Shin and McKay [Shi85] attempts to improve the trajectory
planning stage of robotic manipulator control. The authors claim that robot control is
divided into path plarmer and path tracker components because the process of robot
control, if considered in its entirety, is complicated. Dividing the controller into two
parts makes the process simpler. The path tracker is generally a linear controller and the
nonlinearities of manipulator dynamics frequently are not taken into account at this
level. The simplicity obtained from this division comes at the expense of efficiency and
the source of this inefficiency is the trajectory planner. The authors assert that to be able
to use the robot at maximum efficiency the trajectory planner must be aware of the
robot's dynamic properties. To partially alleviate the inefficiency of the planner the
authors present a solution to the minimumtime manipulator control problem' subject to
constraints on its geometric path and input torques/forces. The output of the planner is
the true minimumtime solution. ' " • '"" ' '
The problem is set up by defining a dynamic model for the manipulator using the
Lagrangian formulation. The motion is constrained to a fixed path in joint space and the
path is given as a parameterized curve f(k), where X varies from to X,max. The derivative
of A, with respect to time is taken as ju. The resulting equation is in terms of X and //. If
22
the parameter X is considered as arc length in Cartesian space, then // is the speed and /i
the acceleration along the geometric path. After setting up the dynamic equations, the
control problem is addressed. From optimization, the minimum cost is equated with
minimum time, which results in optimizing the operating speed of the robot. This is
called Minimum Time Path Planning (MTPP).The cost function is optimized with the
constraints on the velocities, slope of trajectories, and joint torques, as equalities or
inequalities, thus yielding regions of admissibility and islands of inadmissibility. These
regions provide the acceptable bounds for trajectories. Then an algorithm called the
Algorithm for Constructing Optimal Trajectories (ACOT) is used to determine the
optimal trajectories. The optimal trajectories are determined by using the algorithm with
the bounds determined previously using MTPP. The optimal trajectory may actually
touch the boundary of the admissible region, generating a dangerous case. It is pointed
out however, that if slightly conservative torque bounds are used in the calculations, then
the actual admissible region will be slightly larger than the calculated admissible region,
giving some margin of error.
Previously, Luh et al. [Luh77, LuhSl] had proposed the use of linear and
nonlinear programming in trajectory planners to generate desired positions, velocities,
and accelerations. These methods assumed that the desired path was given in terms of
the path's endpoints and set of intermediate points or comer points. Along each segment
of the path the (constant) maximum accelerations and velocities were given. In general,
these constant bounds may have been quite inaccurate for some parts of the segment,
since worstcase bounds for the whole segment had to be used. Shin et al.'s [Shi85]
The problem of moving a manipulator in minimum time along a specified geometric path.
23
method was an improvement over these techniques since it calculated the acceleration
and velocity limits directly from the path, the manipulators dynamic equations, and
actuator characteristics.
Another approach was taken by Shin and McKay [Shi86a] in an attempt to
address some issues that were not fully addressed by the MTPP method [Shi85]. The
authors point out that in the MTPP approach, the solution works only for minimumtime
problems. In situations where driving the robot consumes large amounts of power, the
assumption that minimum time is equivalent to minimum cost may not be valid. Another
assumption is that joint torques can be changed instantaneously. Also, sometimes the
actuator torque limits are dependent on one another as in the case when a robot uses a
common power supply for the servoamplifiers for all joints. To rectify the shortcomings
of the previous approach, the idea of using Dynamic Programming was proposed to find
the optimal phase plane trajectory {X vs. /J). The advantage of using Dynamic
Programming over previous methods that used linear or nonlinear programming [Luh77,
LuhSl] is that it places few restrictions on the cost function that is to be minimized.
Putting limits on jerk is possible, and the interdependence of torque bounds can be
handled easily.
The authors call this approach the minimumcost trajectory plarming (MCTP).
The obstacle avoidance problem is bypassed as it is assumed that the path has been
specified a priori and the problem becomes: what controls will drive the robot along a
specified curve in joint space with minimum cost, given constraints on velocities, control
signals and their derivatives? As done in their previous work, the authors reduce the
complexity of the control problem by introducing a single parameter that describes the
... . ,' ■■■■ *. ; .  ^ ' ■ _ ^ ■
robot's position in the form of a parameterized curve in the robots joint space. This
single parameter and its time derivative completely describe the current state of the robot.
The problem then becomes a twodimensional minimumcost control problem with some
state and input constraints.
To apply dynamic programming to this problem, in which the dimensionality has
been reduced to two state variables, the phase plane is divided into a grid. The cost of
going from one point in the grid to next point is calculated. Since all the dynamic
equations are strictly given in terms of the state variables, the cost computation is done
entirely in phase coordinates. Once costs have been computed, the dynamic
programming algorithm is applied to determine positions, velocities, and torques, from
the resulting optimal trajectory, and the curve and dynamic equations.
The significance of the results is that they provide a simple approximation to the
truly optimal trajectory problem to any desired degree of accuracy. It must be noted
however that the effect of grid size on the quality of results is of practical importance.
The grid should be fine enough to provide good results but not too fine to cause excessive
computations. The varying of column size and row size in the grid will yield different
effects on the results. Varying the number of columns (k divisions) varies the accuracy
of the dynamic model while varying the number of rows (ju divisions) varies the accuracy
of the approximation to the true minimumcost solution. Another important factor is the
ratio of the number of rows to the number of columns so that the slope of the curve
connecting two adjacent points in the grid is small.
^ The task planner generates a sequence of Cartesian points and additional intermediate
knot points which, if necessary, are transformed pointwise to points in joint space. These
joint points are interpolated to form a geometric path in joint space [Pau81].
25
Another paper authored by Shin and McKay [Shi86b] addresses the issue of
finding the geometric path. The problem states that given the solution to the minimum
time trajectory planning problem, find the geometric path or the trajectory curve so as to
minimize the traversal time.
Xiangrong and Xiangfeng [Xia94] present a method for trajectory planning that
includes Cubic Polynomial (CP) motion trajectory plaiming and addresses the minimum
time trajectory problem. Their work extends and improves the work done on a similar
problem by Luh et al. [Lin83]. It uses Cartesian space schemes for trajectory planning.
The characteristics of CP motion are that the endeffector not only must achieve the final
target point, but also must move along the desired path. The time history of the positions,
velocities, and accelerations of the endeffector are assumed to be prescribed, and the
corresponding joint displacements, velocities, and accelerations are obtained from a given
position and orientation of the endeffector by solving the inverse kinematics.
If T is the time needed for a robot to perform a CP motion, the time interval 0T is
divided into smaller subintervals. At each interval computations are done for the
transformation matrix for the endeffector coordinate with respect to the base coordinate
using the equations of the path and constraint conditions for the endeffector position and
orientation. Inverse kinematics is used to determine the velocities and accelerations of
each joint. After this information is obtained for two successive intervals, a 3^'' to 5*
order polynomial is used to specify each joint trajectory in that subinterval. If there are
no constraints on velocities of each joint except at initial and final points, a cubic spline
fit can be used for all intermediate points and a 4"" order for the first and last point. The
26
coefficients of polynomials at each segment can be obtained recursively and hence the
trajectories for all subintervals are planned.
The next step is the MinimumTime Trajectory planning. It is assumed that the
maximum velocity, acceleration, jerk, and torque for all joints are specified and are used
as constraints. A new scaling factor is determined which attempts to maximize (»1) the
ratio of current value/maximum value (of velocity, acceleration, etc) during a sub
interval. A new trajectory is determined recursively using the former method until the
maximum ratio is obtained at which point the minimumtime joint trajectories are
considered obtained.
The paper discusses an additional issue of determining the number of time sub
intervals. A maximum allowable deviation of trajectory is assumed. Using the CP
Trajectory algorithm the trajectory is planned. From this information the corresponding
actual position of the endeffector in Cartesian space is determined. This is compared
with the trajectory, and if it is not within the allowable deviation, the subinterval is
divided into two parts by adding an intermediate point and the same process is repeated
until an acceptable time interval is obtained.
Piazzi and Visioli [Pia97] have presented an approach to find the minimumjerk
cubic spline joint trajectory of a robot manipulator using interval analysis. Minimum
jerk trajectories are desirable for their amenability to path tracking and to limit robot
vibrations while cubic splines are used to ensure continuity of velocities and accelerations
in the robot movement.
The importance of minimizing the jerk in trajectory planning is primarily due to
the fact that joint position errors increase when the jerk increases. Moreover, minimum
27
jerk joint trajectories are desirable for their similarity to human joint movements and to
limit robot vibrations. For achieving this goal, a global optimization method, based on
interval analysis, is presented.
The path of the manipulator is specified in Cartesian coordinates and is converted
to joint space via inverse kinematics. The sequences of joint displacements are
interpolated by use of cubic polynomials to ensure an overall continuity of position,
velocity, and acceleration. Using the total elapsed time for the displacement, a
parameterized spline is obtained that incorporates continuity of positions and velocities.
The optimal trajectory planning problem with minimumjerk criterion is then posed as a
constrained minimax optimization problem. The Interval Algorithm [Moo79] is used for
the global optimization problem and is called the MinimumJerk Interval (MJI)
algorithm. An example is presented for a 2D0F robot to illustrate the minimumjerk
jointspace trajectory planning with continuity of velocities and accelerations using the
interval algorithm. Vv I :•
According to the robot's dynamic model that is employed, trajectory planning can
be done either continuously or discretely. Techniques such as of [Shi85] employ a phase
plane technique to obtain the optimal solution for the trajectory planning problem. In
these algorithms, constraints are considered to make full use of the capacities of the joint
actuators and a true minimum time solution is yielded. It should be pointed out that these
algorithms work for a small range of trajectory planning problems [Du95] that consider
only generalized force constraints. Other realistic constraints such as joint velocity
limits, jerk constraints, and endeffector velocity limit are neglected.
28
A discrete model is, however, more appropriate than a continuous model in
solving the trajectory planning problem [Fla88]. In this method the whole generated path
is discretized by individual points. The obtained trajectory of the discrete points that
optimizes the given performance index is thus an optimal one. The trajectory planner
presented by Flash [Fla88] is based on a linear programming model. The use of an
objective akin to the minimum time objective rather than the true minimum time
objective function makes it possible for linear programming to be used, which allows fast
computation. However, in this method, robot dynamics are not considered. Tan and
Potts [Tan88] extend this method by considering nonlinear constraints, such as
generalized forces and joint jerks, and use nonlinear programming to obtain the optimal
solution. In their method, the discrete trajectory plaimer is modified by taking all time
intervals to be the same, which is difficult to execute as it is computationally extensive.
A new method for segmental discrete trajectory planning, in which the whole
trajectory is planned segment by segment along the path is presented by Du et al. [Du95].
The consistency of the time interval at the trajectory planning stage with the sampling
time interval determined at the trajectory tracking control stage is considered. The
method reduces the computation memory and run time.
Path planning is often times categorized as either being static or dynamic,
depending on the mode of available information. Static path planning is used when all
the information about obstacles is known a priori. Most path plarming methods are
static. Dynamic path planning is used when only partial a priori information is available
about the obstacles, and the environment is unpredictable and timevarying. Zelek
[Zel95] has presented an approach for dynamic path planning. Dynamic path planning is
29
problematic because the path needs to be continually recomputed as new information
becomes available. Dynamic path planning is accomplished by using a harmonic
function as the potential function to model the free space. Computation of the harmonic
function is formulated as an iterative process and is performed independently of the
execution of trajectory commands. In order to achieve continuous robot navigation with
guaranteed control, there are coarsetofine set of harmonic computations being
continuously carried out in parallel.
Techniques to Improve Path Tracking
As alluded to before, since robot dynamics are highly nonlinear, coupled, and of
high dimension, timeoptimal control is difficult: to simplify the problem it is commonly
divided into offline trajectory planning and online tracking. The papers reviewed above
have addressed only the trajectory planning problem, and have done so considering only
the robot dynamics and not the tracking controller dynamics. These solutions have used
the path constraint to reduce the dimension of the problem to 2, the path position and
velocity, and solve the problem using a variety of optimizing techniques.
These solutions provide mathematically correct timeoptimal trajectories for any
given robot model. However, their application to real systems is not so straightforward.
Because the robot dynamics are never precisely known, the resulting trajectories are too
approximate to be applied openloop. The path constraint, which is so useful in reducing
the dimension of the optimal control problem, preempts consideration of tracking error.
The resulting timeoptimal solutions push actuators to their limits, leaving no margin for
controlling tracking errors.
30
Some published results have addressed these issues, including an approach by
Shin et al. [Shi87] that proposes planning trajectories that are robust to payload
uncertainties. Cahill, Kieffer, and James [Cah96] proposed a scheme for planning robust
"timeoptimal" trajectories for robots under computedtorque control. This scheme was
based on the experimental identification of unmodeled dynamics as joint acceleration
disturbances and a theory for relating those disturbances to torque margins that should be
held on reserve during trajectory planning. The scheme also involved the selection of
controller gains to ensure that tracking is accurate to a prescribed tolerance.
Kieffer et al. [Kie96] in continuation of their previous work [Cah96] extend the
approach to a more general architecture for online trajectory generation. Their work
proposes a state feedback law that ensures nearly timeoptimal path tracking which is
accurate to a prescribed tolerance in the presence of experimentally identified levels of
disturbance. The input path acceleration (s) is generated online as a feedback function
of the plant and trajectory generator states and is thus called a closedloop trajectory
generator.
The model for the robot is developed which is inclusive of joint acceleration
disturbances. These disturbances can be identified from online measurements. The
tracking error is then predicted and compensation torques are computed based on the
error. A methodology is developed for choosing controller gains to meet a prescribed
tolerance for tracking accuracy and for predicting levels of compensation torque that will
develop online. With the theory developed to this point, the authors had previously
employed dynamic programming to robustly determine timeoptimal trajectories
[Cah96].
31
In the current instance, they extended it to Robust ClosedLoop Trajectory
Generation. The idea is to determine how much the path acceleration {'s) can be
modified without saturating any actuators which is necessitated by the fact that the
equations predicting the tracking error and compensation torque are only valid if the
torque commands are implemented without clipping.
The closed loop architecture was implemented on the first two links of a SCARA
robot arm. The robot model, identified by least squares techniques, included all rigid
body inertial effects as well as independent coulomb and viscous friction terms for each
joint. The task was chosen to be the same as the one used when the previous technique
was implemented for comparison purposes. The results were successful, as the path
velocity of the closedloop scheme was everywhere greater than or equal to that of the
previous scheme. The total elapsed time for traversing the trajectory was also improved
and was very close to the model's timeoptimal elapsed time. Torque utilization also
increased substantially and exact saturation occurred most of the time. Also, the closed
loop system tracked the path within the specified tolerance, and that the errors were very
similar to those of the previous robust trajectory planning scheme. It was concluded that
the tracking times were reduced while the tracking accuracy and robustness remained
approximately the same. Keiffer et al. have presented both the techniques with extensive
experimental results in [Kie97]. ; .. ? * ^ v • ^
, Another approach to address the tracking problem involves developing methods
for path planning and path tracking simultaneously. This approach is taken by Tsuchiya
and Egami [Tsu90], and they assert that the best whole system is synthesized by
combining them suitably. They have proposed robot path control with variable speed
32
trajectory planning and an assortment of control methods for tracking. They present a
new trajectory plarming method that takes the control ability of the designed manipulator
control system into consideration. This is done systematically without trial and error on
the basis of the frequency response of the designed control system. This trajectory
planning method is implemented with the following control methods: Adaptive control,
Preview control, and Zeroerror tracking control.
An important issue at the trajectory plaiming stage is how to utilize the freedom of
tracking speed. The purpose of the trajectory plarming method in this paper is how to
carry out trajectory planning so that the system can utilize the maximum ability of the
designed control system. The results show that the tracking speed of the robot
manipulator in both the work space and joint space changes with the curvature of the
given path. This trajectory planning method is known as "variable speed trajectory
plarming." The method proposed by the authors differs than the traditional variable speed
trajectory plarming in the sense that the ability of the designed control system is explicitly
taken into account. ~ *^
The methodology for the planning entails a series of procedures and starts by the
synthesis of the manipulator control system. By using the Bode plot of the control
system the maximum angular frequency that guarantees no gain deterioration and phase
delay is determined. The minimum angular frequency (coo) amongst each joint servo
system is determined. This is referred to as the standard angular frequency and the robot
can track the path with an angular frequency lower than this. The given path in Cartesian
space is then converted into joint coordinates. A reference point sequence is obtained by
plotting equidistant points on the transferred path by assuming that the robot manipulator
33
tracks the path with a constant speed. A circle is obtained consisting of three consecutive
reference points on part of the tentative desired trajectory. From the circle, the angular
frequency of the sinusoidal signal (odi) to be applied to the servo system to draw the
circle is obtained. The points are taken as reasonable reference points on the path if coi=
(Oo, otherwise the points are moved in appropriate direction until the condition is satisfied.
A circle consisting of three consecutive points approximates each part of the given path.
By using this method a desired signal is obtained as a function of time that can be applied
to each servomotor; this signal is always part of a sinusoidal function with fixed angular
frequency.
Utilization of future information is useful for improvement of system response if
available, and usually, the path tracked by a robot is available in advance. In this context.
Preview Control utilizes future information and has many attractive characteristics over
usual control systems. Although even simple application of preview action is effective,
application along with variable speed trajectory planning is even more effective in
reducing tracking error. In order to determine the desired signal for each servo system of
each arm, trajectory planning is carried out based on the frequency response of preview
control. A characteristic of this control system is the existence of feedforward
compensation utilizing a future desired signal. The transfer function of the preview
control system from desired signal to controlled variable is obtained and the maximum
angular frequency from the Bode plot is determined. The servo system can respond to
the desired signal with angular frequency coo without any gain deterioration or phase
delay.
34
Another control method employed with variable speed trajectory planning is the
Zeroerror Tracking controller. Zero phase error tracking controller is proposed to cancel
the effect of unstable zeroes of a stabilized closedloop system. A unity gain controller
can be derived on the basis of zero phase error controller when the angular frequency of
the desired signal for the control system is known and fixed. In general, this type of
controller is not useful because such a situation rarely exists in usual engineering
problems. However, the condition of constant angular frequency of the desired signal is
always satisfied by variable speed trajectory planning. Nonlinear compensation methods
are further employed to eliminate the nonlinear effects.
The authors propose a third method for controlling the robot that employs the
variable speed trajectory planning method. Adaptive Control is used as a countermeasure
to parameter variations of the robot manipulator during an operation. Specifically, model
reference adaptive control (MRACS) is applied to robot path control. By using the
MRACS the control system is decoupled. This decoupling is very convenient for
application of variable speed trajectory planning method as the method is essentially for
singleinput singleoutput systems. It requires plotting of Bode plot for each servo
system.
Simulation results for the third method are presented. It is noted that the results
for the other two control techniques were omitted, as the results were more or less the
same. Adaptive control (MRACS) is used with constant speed and variable speed
trajectory planning. It is noted however that robot path control is considerably improved
by applying the proposed trajectory planning with the significance of the said method
being its nonreliance on trial and error methods. The input torque restriction of the
35
servomotors is not dealt with and reference is made to the work of Shin et al. [Shi85] that
addresses this issue.
Abe and Tsuchiya [Abe92] pubUshed work that refined and detailed the work
published earlier on variablespeed trajectory planning by Tsuchiya and Egami [Tsu90].
Wang, Tsuchiya, and Hshimoto [Wan93] presented two control methods for path
tracking: a digital control method based on the principle of movement of a nonlinear
mechanical system, and a preview control method utilizing future information of the
desired trajectory to improve the tracking performance of the former. A digital control
law was derived and disturbance terms (functions of position and velocity) were included
that otherwise may have been unknown.
Wang and Tsuchiya [Wan94] proposed a new trajectory planning method based
on highlevel qualitative judgement. Fuzzy reasoning was used as the tool for this. The
method is called fuzzy trajectory planning. This effort was to address unresolved
problems in the previously proposed variablespeed trajectory planning method.
The proposed trajectory method takes into account the important efficiencies and
limits of a robot manipulator. Fuzzy reasoning [Zad69] is considered a powerful tool for
making brief judgements and with the algorithm being very simple, it is used for the
trajectory planning problem at hand. The requirements for setting the trajectory planning
are set as:
• The attained trajectory must be efficient for calculation and for putting into practice;
• Its position, velocity, and (except at the starting point) acceleration must be smooth
functions of time; and
• Trajectory planning must be done without any trial and errors, keeping in
consideration the limits of the actuator torques and the characteristics of the control
system.
36
The variable speed trajectory planning suffered a few shortcomings in regards to
the aforementioned requirements. The method required different processes for starting
and stopping, i.e., the planning method had to be changed for the starting and ending part
of the path. The method did not handle the torque limits on the actuators positively, and
the algorithm was too complex. The fuzzy method proposed attempts to solve these
issues at a certain level.
In fuzzy trajectory planning, the rough knowledge of the robot manipulator and
the control system is first expressed at the language level. Then by using a membership
function of the fuzzy set, these expressions are transformed into fixed quantities. And
then trajectory planning is done using the fuzzy reasoning method. Therefore, the most
important step in fuzzy trajectory planning is how to acquire the rough but important
knowledge about the robot manipulator and the control system, and how to make them
abstract.
Abe and Tsuchiya [Abe92] have already made use of the frequency characteristics
of control systems and have explicitly taken the efficiencies of the control systems into
account. The same information is adopted for fuzzy reasoning and for setting up a rule
base. Next, the issue of velocity and torque limits is addressed. Once again, the variable
speed trajectory planning provides the solution so that the velocity limits are not
exceeded. The velocity of a robot manipulator moving along a path in a joint angle space
can be determined by three items: the radius of curvature, the angular frequency of the
trajectory, and the distance L from a sharp bend. All these are treated as fuzzy variables
and constitute the fuzzy reasoning rules.
37
Once all the information is available at the language level, it is expressed in
qualitative form by using membership functions of fuzzy sets. The rules for reasoning
are set up. By using the rules, the reasoning results are obtained followed by
deflizzification to get the final solution for the velocity to move along the path. While the
issue of limiting torque is not addressed, it is pointed out that the information obtained
from the reasoning results can be used to calculate the required torque. If this torque is
within the prescribed limits then no action is needed. If not, then a search for a suitable
new point near this point is done by moving along the path, taking very small steps.
From the new point, the trajectory planning is carried out again by using flizzy reasoning.
The authors concede that the problem of torque limits is a difficult problem to
handle systematically at the trajectory planning stage and that although their method does
consider torque limits, there may be cases where it exceeds the torque limit. The way to
correct is to check for the exact value of torque, and if it exceeds the limit, reflect it
immediately in trajectory planning. This will increase the processing time but the time of
calculation for the fuzzy method is shorter than the variablespeed trajectory method.
In summary, this method can obtain the trajectory without trial and error by
considering the efficiencies of the control system and motor limits. It has a fairly simple
algorithm, which, in some cases, may allow trajectory planning in real time. Also, the
position, velocity, and acceleration form a smooth fianction in time.
Grasa, Volberg, and Polyakhov [Gra93] present an approach that combines direct
adaptive control and fuzzy logic control. To determine the coefficients and terms of the
robot dynamic model and unmodeled dynamic effects, the authors use a combination of
the computed torque method and the velocity gradient technique. This provides a simple
38
way to obtain some known and unknown parameter adaptive control laws, in particular,
the exponential path tracking adaptive control algorithms. The algorithms require online
measurements of the tracking error and its first derivative. The fuzzy logic control
strategy is employed to attain implementable algorithms based on the developed adaptive
laws.
Many modelbased controllers neglect nonlinearities or system dynamics to
simplify the design [Mas99]. These omissions may lead to poor performance or
instability. This is especially true for MultiInput MultiOutput (MIMO) systems.
Mason, Walchko, and Crane [Mas99] have presented the development and
implementation of MIMO hybrid fuzzy controller for a twolink robot manipulator to
deal with the problem of designing MIMO control laws for systems with significant
nonlinearities in the dynamics. The controller is aimed at providing better tracking
performance of the robot manipulator. The method utilizes the designer's intuition
(heuristic knowledge) and the mathematical description of the system. The controller
combines the traditional proportionalderivative (PD) control law with a fuzzy controller
in a MIMO framework. This is done because hybrid controllers provide a more defined
control structure over fuzzy control while they improve the accuracy and portability of
the controller.
The MIMO fuzzy controller is formulated by extending the SISO (singleinput
singleoutput) fuzzy concepts to the vector domain. The control vector is resolved into a
direction, magnitude, and positionderivative ratio and by doing so the MIMO problem
for solving for a control effort is reduced to a scalar one. The system is further simplified
39
by making assumptions about the system (e.g., uncoupled), and the complexity of the
fuzzy inference system is reduced. i
For the purposes of benchmarking, a PD and a Feedback Linearization (FL)
controller are also developed. Simulations are conducted to examine the performance of
a fuzzy MIMO hybrid controller in ideal conditions. These results are compared against
the PD and the FL schemes. Another set of simulations is done to study the effects of an
unknown load disturbance. In the first case, the FL and the PD schemes produced a
larger steady state error than the fuzzy scheme and their control effort was higher too. In
the latter case, with random and deterministic disturbances, the fuzzy controller was
better able to account for these disturbances. The control effort in this case was the same
for all controllers. It was concluded that the MIMO fuzzy controller outperformed the
other controllers. This scheme is more robust to random and deterministic disturbances
and also offers portability to other robots with minimal effort.
Najson and Kreindler [Naj96] have proposed a discontinuous robust control law
for rigid manipulators to track a desired trajectory. The proposed control laws exploit
manipulator properties and do not require the measurement of accelerations and the
inversion of the inertia matrix. They require the knowledge of the present value of the
desired trajectory and its first and second derivatives, rather than an entire predetermined
trajectory for a given task. This enables online task changes and model following can be
accommodated.
The authors set out by deriving the dynamic equations of a rigid manipulator.
The equations are transformed to a form suitable for adaptive control as established by
Slotine and Li [Slo87]. A robust tracking control law is derived for the discontinuous
40
case, which is a statefeedback control law. The global, exponential stability to zero
tracking error of the law is proved by a Lyapunov method and has exponential stability.
A continuous approximation of the control law is then developed. This is done to avoid
chatter in the system; while this causes the error to not exponentially tend to zero, it
causes it to ultimately converge to an arbitrarily small neighborhood. This is what is
referred to as practical stability. Once the controller is designed, adaptive ideas are
incorporated. The gains are determined adaptively instead of being fixed in advance.
An example is presented with simulation results for a twolink manipulator with
parameter and disturbance uncertainties, including motor dynamics and friction. The
continuous control law showed excellent robust tracking. The tracking error was reduced
to near zero in about 0.8 seconds. It is noted that the control law does not take into
account the uncertain motor dynamics added in the simulation.
Dakev, Chipperfield, and Fleming [Dak96] have presented an approach to
determine approximate solutions of path following optimal control problems by
exploiting modem global search and optimization techniques. According to the
methodology developed, controls are represented by discrete vectors and substituted in
the system equations. Alternatively, when it is possible to exclude the controls from the
system equations, a discrete vector represents the parameterization function of the path.
In either case, the components of these vectors are regarded as variables of a performance
index based goal ftinction that is to be minimized with respect to the control and phase
constraints.
Employing such schemes allows the modeling and solution of both openloop and
closedloop path following optimal control problems within a unified framework of
41
function approximation and constrained optimization techniques. It allows for
implementation of genetic algorithms for global optimization, multiobjective control, and
utihzation of parallel processing.
An optimal path following algorithm for control of multibody systems is
developed, and is tested for minimumtimeenergy control of twolink manipulator. The
path following optimal control problem is reduced to a constrained optimization problem
by introducing a parameter set in order to approximate the control inputs. In the open
loop case, controls are regarded as the function of the time variable, t, while in the
closedloop case they are considered as functions of the state variables. Consequently,
the original openloop optimal control problem is reduced to a nonlinear programming
case and the closedloop problem is reduced a standard optimization problem. Once the
problem is set up, the goal function and associated constraints are evaluated and the
global optimum of the constrained optimization problem is found. In the case of closed
loop control, identification of the control law based on the input/output relations obtained
from the solution of the openloop optimal control problem needs to be done. The
methodology was tested by simulating a model of a robot with one revolute and one
prismatic joint with both openloop and closedloop optimal control.
Li, Tso, and Zhang [Li98] have proposed a dualmodelbased structure for
uncertainty attenuation in the trajectorytracking control of robot manipulators. The
traditional advanced control schemes are model based in the sense that a dynamic model,
described either explicitly as a mathematical equation or implicitly as an inputoutput
relation, is used in their design or final implementation, as in [Slo87, Tsu90]. The design
of this type of model based control (MBC) is usually achieved in two steps: an a priori
42
estimated dynamic model of the plant is first implemented in the control structure to
linearize plant dynamics, and another controller is then designed for the resultant
simplified dynamics to achieve a desired performance.
The application of MBC is however hindered by a practical problem: how to
determine the actual dynamics of a complicated, highly nonlinear, and highly coupled
robot manipulator. Some of these issues can be resolved by modeling the manipulator
links as rigid bodies, however the problem of varying payloads, unknown effects of
friction, external disturbances, remain. The improvements to deal with such issues have
come in two broadly generalized areas: adaptive MBC and robust MBC. In the adaptive
MBC structures, an adaptation mechanism is applied to capture the payload variations
and compensate for the unmodeled dynamics and external disturbances. The robust
MBC approaches employ a fixedstructure control algorithm in their configurations to
generate a robust performance with low sensitivity to modeling uncertainties. Compared
with adaptive approaches, the robust approaches are simpler to implement and require
less computational load. ^ ' "
The authors assert, based on the current state of control technology, that an
improved controller scheme should lead to satisfying the following features: the control
performance is insensitive to modeling errors and external disturbances; no high feedback
gains are utilized in the design; the modification structure is simple and the performance
analysis straightforward. In lieu of this assertion, the authors have presented a dual
modelbased control (DMBC) structure for the trajectorytracking control. This structure
consists of two prime elements: a basic control structure and an uncertainty attenuation
compensator.
43
The basic control algorithm is first applied to modify the robot dynamics so that it
becomes an openloop stable system. The authors then select the robust approach in the
form of internal model control (IMC) structure to further compensate for uncertainties. It
is stated that this DMBC approach is an enhanced scheme of the conventional robot MBC
algorithm, with the IMC outer loop added as a complementary structure. Simulation
studies were performed to demonstrate the effectiveness of this method. A 2link (2R)
manipulator was used with the basic control structure as well as with the additional
compensation structure. The results showed that the effects of modeling uncertainties
(comprised of parameter errors and unaccounted frictional effects) were significantly
reduced by the DMBC approach. Similar results were obtained while studying the effects
or external disturbances. It is noted however, that before the disturbance errors were
introduced, both systems had tracking errors due to quantization error in the simulation.
Of importance is the fact that the DMBC errors are 10 times those of the single
controller. This is attributed to the side effects of the filter dynamics, which can be
reduced by choosing a smaller filter time constant at the expense of higher sampling
speed.
In another approach combining path planning and tracking control of a robot
manipulator, Park and Park [Par97] have suggested the use of neural networks. This
approach is used to deal with the presence of system uncertainty and obstacles and
employs the use of neural networks combined with a conventional feedback controller.
The path planner provides not only the (sub) optimal trajectory for a given cost function
but also the configurations of the robot manipulator along the path by considering robot
dynamics. The path is adjusted to avoid singular points and obstacles. An additional
44 .
neural controller compensates for the tracking errors caused by uncertainty and
disturbances, which provides robustness with a good tracking performance.
Amin and Ahtiwash [Ami96] have proposed another neural network based
scheme. The authors employ two neurocontrollers utilizing the back propagation
algorithm for robot tracking performance: the Inverse NeuroController and the Neuro
Emulator NeuroController schemes. For a given task of moving a robot from an initial
rest position to a final specified position in minimum time, the profiles for position and
velocity are determined. The results indicate that the two neurocontrollers allowed
smooth and accurate motion of the manipulator.
CHAPTER 4
PATH TRACKING METHODOLOGIES
The following material presents an investigation into the development of path
tracking methodologies and their application to autonomous robots, serial manipulators,
and parallel platforms. The underlying assumption throughout the work is that the
desired continuous path is known as a function of time, and if only discrete points along
the path are known, a continuous approximation can be generated.
These approaches were developed in part from inferences made from preliminary
attempts at path tracking techniques that are presented in Appendix A. The results of the
simulations based on these earlier approaches, while not being very promising, did
provide insightfiil information. It appeared that the mechanism observed a chaotic
behavior and did not converge to a solution. Although the accelerations and velocities
were quite high and discontinuous, it was anticipated that the error could be reduced and
the solution would converge. This unexpected behavior was quite peculiar and lead to
some conjectures:
• The behavior could have been the result of the method being based purely on a
proportional controller type algorithm i.e., correction was attempted purely for error,
and with no damping in the system, stability could have been an issue.
• Another reason for this type of behavior could have been the interaction of the links
of the manipulator with each other. A solution to this problem was testing the
algorithm for an autonomous robot case so that there is no interaction amongst the
links.
Based on these observations, the basis of path tracking is developed first,
followed by path tracking for autonomous robots, serial manipulators, and parallel
45
46
platforms. The developed algorithms were implemented in simulation and their results
follow each section.
Screw Theory Based Path Tracking
The problem of a rigid body following a path with a desired pose (position and
orientation) is considered first. Consider the situation described in Figure 4.1. The
desired path of the rigid body B is given by the timed path of point 0, and the angle 9(t),
which is the angle between the xaxis of a coordinate system fixed in rigid body B and a
fixed reference. It is assumed that at some time to, the current pose of body B is given by
Bc(to). It is also assumed in this case that only pose information is available.
PATH Desired pose at time (to)
Yc ^N°^^"""""''~~^ ^ vA^kS"^ Desired pose at time (t„+dt)
Oc(to) Xc"" '^ ..vo^^^"^ '^X
Current pose at time (t„) \
Figure 4. 1 : A rigid body following a path with a prescribed pose.
The problem is to devise a technique so that the error between the current and the
desired poses is either eliminated or minimized to within a certain threshold. Chasles'
Theorem [ChaSO] will be used as the basis of undertaking this problem. Chasles'
Theorem states that given the two positions of a rigid body, there is a unique screw
47
displacement that moves the body from one position to the other. This result will be used
to investigate a variety of techniques to solve the problem.
The solution is first attempted without regard to time, i.e., time is of no
consequence in the desired displacement. From Figure 4.1, the following positions are
considered:
• The current pose at time to, and
• The desired pose at time to
Using these two positions, Chasles theorem ensures that there is a unique screw
displacement that moves the rigid body B from the current position to its desired position.
Let this screw displacement be denoted by the screw, $^(, j^^^, , . Further let AG be the
rotation angle and h = As/ ^0 be the pitch associated with it, where As is the linear
translation along the screw axis. Finally, assume that the screw displacement will take
place in a time interval At. Then, assuming that the displacement takes place at a
constant angular velocity, the angular velocity associated with the rigid body (or the end
effector) is given by:
A0
""^^ (4.1)
Further, the constant velocity state associated with the rigid body B (or the end effector,
in case of a serial manipulator) is given by:
The final result of this path tracking technique is that at time to+At, the rigid body B will
have the desired position at time to. Thus, timewise, the tracking will be behind the
48
desired tracking by an interval of At. Alternatively, it can be asserted that body B will
track the desired path with a lag.
Extending the above case and considering time as a factor, a rather simple way to
avoid the lag or time delay of the previous technique is as follows:
• The positions to be considered are: The current pose at time to and the desired pose
atto+At.
• To require that the resulting screw displacement will take place in the time period
At.
The first condition will determine a different screw, $c(,j></(,„+a/) » ^ different
rotation angle A9, and a different pitch h=As/A9. The second condition would yield that
the constant angular velocity, associated with the screw is,
and the constant velocity state of the rigid body (or the endeffector for a serial chain) is
given by
^ = «$c(,„w(,„.A/) (4.4)
This technique ensures that at time to+At, both the current and the desired positions
coincide. Whatever the employed technique, if the rigid body B is the endeffector of a
nonredundant spatial serial chain, ybr example, the corresponding joint velocities can be
computed as follows (Figure 4.2):
A closer look at these techniques obviates the fact that these techniques would
produce commands
49
• with discontinuous angular velocities and
• with discontinuous locations of screw axes.
Figure 4.2: A serial manipulator traversing a prescribed path.
These discontinuities will produce infinite accelerations, and obviously
undesirable characteristic for any robotic system. Additionally, there is another quite
subtle source of a problem. It should be noted that in the case of a serial manipulator,
Equation (4.5) can be used to calculate the joint velocities ,6>,+i,(/ = 0,1,2. .,5) . However,
in the time interval At, there will be position changes that will affect the screws '$'^'.
Thus the joint velocities are only accurate at t==to. This however, might not be a major
source of problem as the transient error may be reduced to an acceptable threshold by
varying At (making it smaller).
50
Autonomous Robot / Vehicle
Considering Pose Information Only ;
Now, the problem of an autonomous robot following a path with a desired pose
(position and orientation) is considered as shown in Figure 4.3. The desired time
dependent path, ( Xj(t),yj(t);0j{t) ) , is assumed to be specified. The current pose of the
body at the instant /p, is given as (x^(/o),>'^(/o);^^(/o))and the desired pose at time
(t, + At) is ( X, (fo + AO, yj (to + A/); e, (/o + A/) ) .
Y A
OAto + ^O
X
Figure 4.3: Autonomous robot path tracking with pose constraints.
Considering the position level only, the following desired velocity screw can be
derived (from Equation 2. 39):
51
V =
(4.6)
where,
0),=
OAL + AOe^it,)
At
^.=
""y^
At
yAto + ^t)yAto)
At
(4.7)
The resultant motion due to this screw will ensure that at time (t^ + At) the
position and orientation (pose) will coincide with the desired pose. It should be pointed
out here that this method assumes that all velocity screws are possible, which may not be
entirely feasible due to vehicular movement constraints. It should be noted that at the
pose level, the commanded motion is continuous at the position level and is a linear
approximation of the trajectory.
Considering Pose and Velocity Information
In this approach, the desired velocity state is also specified in addition to the pose
information. Initially, it is assumed that current pose and velocity information is known.
As shown in Figure 4.4, the current state is defined by:
^c(^o).>'c(^o);^c(^o) 1
t^.c(^o).'^^c(^o);<yc(^o) J
and the desired state at instant {t^ + At) is:
52
»
Using the pose information, the velocity screw, V (4.6), as done previously, can
be calculated as :
a>.
V,
""y
At
At
At
(4.8)
Further, from the velocity information, it is possible to determine the following:
f//o + AO
r,(/o+AO
\ ^ ►
X
Figure 4.4: Autonomous robot path tracking with pose and velocity
information.
«,
a, 
S =
53
o),it,+At)co^{t,)
At
^..(^o+AO
^A(o)
At
u^,{t,+At)
OyA(0)
At
(4.9)
This information, coupled with the velocity state at t^, will provide a reduced
acceleration screw A^ . However, it is important to note that determining how the two
screws, velocity and acceleration, will affect each other is an issue that needs to be
addressed.
The problem is now set up differently in a way conducive to determining the
common action of two screws. Assume that the initial or current pose information (state)
of the body is known. Consider a rigid body B and let (l»^o,l>_^o;«o) denote the initial or
current velocity of the body and let (iJ;fOc'^>'Oci^oc) represent the (to be determined)
required command velocity parameters. Also let (a^,ay;a) denote the acceleration state.
Then after a time step, At , the final velocity and position is given by:
Velocity:
cOf =(0q+ aAt
(4.10a,b,c)
Position:
Oj =0o+o)oAt + ^aAt^
yd =yc+Oyo^t + ^ayAt^
(4.11a,b,c)
54
These equations above are the algebraic equivalent of the common action of a
velocity and an acceleration screw. It will now be shown how the command parameters
(D^oc'^rOcJ^oc) will be calculated. Of course, the calculated command parameters may
differ from the current velocity parameters, and any control system that is applied will act
to minimize this difference. Assuming that the initial pose (j^o»>'oi^o) '^ known and the
final pose (xj^,yf;0j^), the final velocity (v^Qf,Vyf;o)f), and the time interval At are
specified. Equations (4.10) and (4. 11) above are used to solve for co^^ and a as follows:
From Equation (4.1 0)a we have,
a =
At
substituting this in Equation (4. 1 l)a gives.
(4.12)
0f^0o+(OoA( + l
At'
rearranging, we obtain
Oj^ 6q =cOo^At + ^o)jAt—^o}Q^At
= jCOQ^At + jCOj^At
and finally solving for cOq^ ,
2(0,0,)
6>n. = (0
■'Oc
At
f (4.13)
where cOq^ is the velocity for the time step At that would satisfy the pose and velocity
requirements at t + At .
To solve for a , substituting Equation (4.13) in Equation (4.12):
55
a
(Oj_ 1_
2(0,0,)
At
co,
a =
At
«/
2 (0^0,)
At
+ CO,
a ■
2{ {0,0,)\
At
0),
A^
(4.14)
The linear velocity and acceleration is calculated by following a similar
procedure. Considering Equations (4. 10) b,c and Equations (4. 1 1) b,c respectively:
a, =
At
At
Xd=X^+^xOc^( + \
yd^yc+^yoc^(+
At
At'
^l^yf^yOc^
At
At'
In a procedure analogous to the procedure for determining co,^ and a above, the
follov^ing unknowns are determined:
V
l\x , Xq )
4:0c
^^Oc =
A^
V
xf
2(>'/>'o)
A^
V
yf
(4.15)
(4.16)
and
<^x =
At
V
a,, = —
' At
4
""yf
(^/^o) '
At J
AX ]
(4.17)
(4.18)
Once all the unknowns are determined, the desired velocity state and the
acceleration state of the body are known:
56
V.=
"0
r ""
(^x
«</
, d =
, 5 =
fl.
^.d
a
y
^yd
L _i
(4.19)
and the reduced acceleration screw (the reduced acceleration state) is calculated as:
A =
a
Uq COXUq
(4.20)
It should be noted that when pose and velocity information is used, the
commanded motion is continuous at the pose and velocity level, which implies that the
commanded motion is a quadratic approximation of the trajectory.
The results obtained from the analysis were employed for a simulation. The
simulation was done in MATLAB®. The test cases and the results obtained are presented
next.
The developed algorithm was tested with two cases. In both cases the desired
path was a circle. The idea behind using a circular path was that for such a motion, the
instantaneous screw axis(pole) would be the center of the circle and would provide a
means of determining an error; which otherwise would be difficult as mixed angular and
linear units are involved. Using Equations (4.6) and (4.7), the constant angular velocity
is determined to meet the pose tracking criteria. From that information, the location of
the pole or ISA is determined using
_ _ (OXU,
r =r\
m
(4.21)
In the first instance (autonomous.m. Appendix B), the center of the circle was
located at (1,0) and the starting point for the robot was(1.51, .05,0). The results of the
57
tracked path are shown in Figure 4.5a. The axes of the graphs represent the x and y
directions of the plane. The 'o' legend in the center of the circle represents the pole
locations of the instantaneous screw axis at each interval of time A/ . Figure 4.5b shows
a zoomedin version of the pole locus. It is interesting to note that the root locus is a
circle too as opposed to being the center point of the circle. This discrepancy is attributed
to using a difference formula for determining the angular and linear velocities. In the
second instance (autonomous l.m, Appendix B), the circle was located at (1,0.5) and the
initial position was specified as (1.5, 0.5). The results are shown in Figure 4.6a and 4.6b
respectively.
In the second case when both the pose and desired velocity of the robot is
specified. Equations (4.13) through (4.18), which require pose and velocity state
information, are used to determine the angular velocities and constant angular
Path Tracking (pose) : Screw Locations "o"
0.6
0.4
0.2
ff
0.2
0.4
0.6
/'
\ .
\
Start
.1
/
^++^
■++.
+++^f4+++++4+++'""
.++"
o pole locations
+ path
0.6 0.8 1 1.2 1.4 1.6 1.8
2.2 2.4
Figure 4.5a: Autonomous Robot: pose information only. Tracked path
and pole locations.
58
accelerations to meet the pose and velocity tracking criteria. Again, the location of the
poles or ISA's are determined using Equation (4.21).
Two paths were considered in the simulations. In the first instance
(autonomous2a.m, Appendix B), the path was a circle with radius 0.5 centered at (1,0.5).
In this case, the orientation was also variant, i.e., the desired orientation was set to be
tangent to the circle at every instant. The starting position of the robot was set arbitrarily
at (1.54, .45). Figure 4.7 shows the path tracked and the locus of poles. The first pole
appears at quite a distance from the rest of the poles, due to the fact that the starting
position of the robot is off from the desired position. Also shown in the figure is the
position and orientation of the robot at each time period. The second case
(autonomous3.m. Appendix B) that was simulated involved tracking a parabolic path.
The starting point of the robot was set at (0.45, 0.81). The desired orientation changed
as a function of time by (5 x A/ ). The results are shown in Figure 4.8.
0.01
0.005
0.005
Path Tracking (pose) : Screw Locations "o"
1?^
'^T?;!:
O ;
O ;
O I
o ;■■■■
o
o
O I
o
o
o
o
••& f
o
o
o
o
o
o
o
o
O ;
O :
0.01 f On
0.985
^
o pole locations
~ path
o
o
o
I o
I o
■o
o
o
o
o
o
o
o
o
o
o
o
o
o
o
o
o
o
j"
■^
0.99
0.995
1.005
1.01
1.015
Figure 4.5b: Autonomous Robot: pose information only. Pole locations.
59
Path tracking (pose) : Screw Locations "c
)"
1 p
' + + + " ""^ + + + '
o pole location
"
+ ,
+ path
0.9
\ "
0.8
■
*
+
+
+
0.7
+
+
l
+
+
+
+
+
+
+
M
+
+
■•«*■: ,...
+
+
0.5
+
+
+
•
Start^,^^
+
■ ■ ■ ■ ■ ■
+
0.4
+

+
+
+
+
0.3
+
+
+
+
0.2
0.1
y 
1 "^++ + ++a+t±i±l — 1 1 1
0.4
0.6
0.8
1.2
1.4
1.6
Figure 4.6a: Autonomous Robot: pose information only. Tracked path and pole
locations.
Path tracking (pose) : Screw Locations "o"
0.515
0.51
0.505
0.5
0.495
0.49
0.485 
: ooooowooc
o pole location
^ path
0.985 0.99 0.995 1 1.005 1.01 1.015
Figure 4.6 a: Autonomous Robot: pose information only. Pole locations.
1F r
0.8
0.6
0.4
0.2
0.2
60
Path tracking (Pose, Velocity) : Screw Locations "o'
jfo pole locations
^^:^r —
'^ej,
'ejsi
<^
0.2
0.4 0.6
0.8
1.2
1.4 1.6
Figure 4.7: Autonomous robots, pose and velocity information. Poles, tracked
path, and vehicle orientation (box).
Path Traclflng (Pose, Velocity) : Screw locations "o"
1.8
1.6
1.4
1.2
1
0.8
0.6
0.4
0.2

T T f
1 1 1 1 1
pole locations
path

o

o
. ■'•' <i>
start
.'~^ o
° <:>
& °
■ ° ^
o
o
"
Q °
o D

\1
w°° JJ
M
1
L. J 1 1 1 1 1 1 1
0.8 0.6 0.4 0.2 0.2 0.4 0.6 0.8
Figure 4.8: Autonomous robots, pose and velocity information. Poles, tracked
path, and vehicle orientation (box).
61
3R Planar Manipulator
Considering Pose Information Only
A new approach to the tracking analysis for a 3R Manipulator is developed (as
opposed to the approach discussed in Appendix A). Consider the 3R Planar Manipulator
shown in Figure 4.9, with the pose information available, i.e., the current and desired
pose states are known.
The approach utilizes the inverse position analysis of the manipulator to
determine the current joint angles for position, 6^^ , and the joint angles for the desired
position, 0^j . From the results of the analysis, the joint angular velocity is calculated by
using the formula (Equation (4.7)):
^,,(/o+AO^,c(^o)
<y. =
At
(4.22)
Desired Path
Figure 4.9: 3R Manipulator with pose information.
62
To determine the errors in the desired and commanded velocities, the desired
velocity state is determined from the path of the manipulator. Also, from this desired
velocity state, the joint angular velocities are calculated by using the following formula,
the underlying assumption being that the path is continuous and differentiable:
QQ)^°$^+^Q)2^$^+20)J^S^ =V (423)
[j]o) = V (4.24)
^ = UV V (4.25)
where, \j\ is the Jacobian of the manipulator, <» is a vector defining the joint angular
velocities of the manipulator, and F is a vector of the velocity state of the manipulator.
Path tracking of a 3R planar manipulator was simulated using the current and
desired pose information using this methodology. A flowchart of the algorithm is shown
in Figure 4.10. All the link lengths of the manipulator were set as unity. The desired
motion of the manipulator was a circle specified by (0.5 + r cos(0,A'sin(/)). The
orientation was not varied. The algorithm (main4.m, Appendix B) uses the inverse
position analysis to determine a set of joint angles. These joint angles are then used to
compute the command angular velocities. To determine the effectiveness of the
algorithm, the calculated velocity parameters (command angular velocities) are compared
to the velocity parameters that are determined using infinitesimal motion screws (desired
angular velocities). This provides a rough order of magnitude comparison and validation
of the command screw. The algorithm computes the Jacobian of the manipulator, which
coupled with the information about the velocity state, provides the desired joint angular
velocities. The two values are compared to determine the error (Figure 4.10).
63
To illustrate how the algorithm works, an iteration of the procedure is shown from
the algorithm main4.m. The current pose or state of the manipulator is given as
r, =(2,1,0) , <l>,=0
where (j) is the orientation of the end effector. The desired path and velocity for the
manipulator is specified as (a circle with radius 0.2 and the center at (0.5, 0))
r^ = (0.5 + 0.2 * cos{t + At), 0.2 * sin(/ + At), O) , ^^ = ^4 25)
Vj  ( 0.2 * sin(^ + At), 0.2 * cos(/ + At), O) . (4 27)
Inverse position analysis is performed by the function reverse_position_3R at the
current pose at time t and at the next desired pose at instant t + A t . The inputs to the
function are the link lengths of the manipulator and the current position of the end
effector. The outputs of the function are the joint angles of the manipulator and the
Jacobian matrix (as defined in Equation (2.49)). Using the joint angle information at
instant t and t + A t the command velocity screw for each joint is generated using
Equation (4.22) as
^command ^^—' (4.28)
The Jacobian matrix is used to determine the joint angular velocities as dictated
by the desired velocity state, F^ , by using Equation (4.25) as
CO.
Wy.
calculated L J d
(4.29)
The two joint velocities are then compared to verify that the generated command
satisfies the desired velocity constraints. The results indicate that the command velocity
is within the desired velocity range by an average of 0.006% with a standard deviation of
0.02rad/sec. ^
64
Figure 4.11 shows the manipulator and the traversed path. Figures 4.12a and
4.12b, and Figures 4.13a and 4.13b show the actual and desired angular velocities and the
errors respectively. The errors are high at the initial step as the algorithm attempts to
jump to the desired position in a single step. This can be addressed by dividing the
motion of the first step into smaller increments or by considering reducing the error in
more than one step, for example, in Ibd instead of A/ . It should also be noted that this
step is application dependent; i.e., this issue is generally avoidable if the path is pre
planned and the initial starting point for the manipulator is known. ■ .
Given the continuous path
(desired position and orientation)
Perform inverse position analysis
at current and desired pose
9's: current and desired are
known
Determine the command
joint angular velocities
that satisfy criteria at the
pose level
Determine the joint
angular velocities using
the velocity state and the
inverse velocity analysis
Compare results.
*{ The difference between terms is the \*
control error.
Figure 4. 10: Algorithm summary for 3R Manipulator path tracking
using pose information.
65
Tracking of 3R manip. using inv. analysis (Pose)
Figure 4.11: 3R Manipulator path tracking with pose information. Manipulator
and the traversed path.
tllTW
40
Actual and Desired Angular Velocities
1
— omegaOl
omegaOl J
30
— omega12
omega 12,
20
1 ■■ ,.;. , '

— omega23
omega23j
10
10

1

20


30
I :

40

50
fin
I r 1 1 r
1
Figure 4. 12a: 3R Manipulator path tracking with pose information. Actual
and desired angular velocities.
66
Actual and Desired Angular Velocities
0.6
' l\i''
1 1 1 1 1 1
omegaOl
omegaOIji
— omega 12
_ omega12j
l\
0.4
0.2
:
■
— omega23
omega23u ■
■ ■/
^ 11
0.2
1
0.4
0.6
0.8
1
1 \ 1
1 1 1 1 1 1 1 1 1 ~
0.18 0.19 0.2 0.21 0.22 0.23 0.24 0.25 0.26 0.27 0.28
time
Figure 4.12b: 3R Manipulator path tracking with pose information. Actual
and desired angular velocities (magnified view).
Errors in Joint Angular velocities
error wOl
— en'orw12
15
10

enor w23
'
5
■
5

10
  ■
15
20
, \:' r \_ ■.« ' 'i .:■ JK;  '*. ■ ■■
25
30
1 1 1 t 1 1
12 3 4 5 6 7
time
Figure 4.13a: 3R Manipulator path tracking with pose information. Errors in
joint angular velocities.
67
0.04
Errors in Joint Angular velocities
1 ' ■  ' V
I
1
1 ■
1
1
1
0.03
— error w01
 error w12
error w23 .
0.02

0.01
.



0.01


0.02


0.03
'
0.04
•'

0.05
1 1 1 1 1 1 1 1
1
0.198 0.2 0.202 0.204 0.206 0.208 0.21 0.212 0.214
time
Figure 4.13b: 3R Manipulator path tracking with pose information. Errors in
joint angular velocities (magnified view).
Considering Pose and Velocity Information
The above analysis is further extended to consider the case where the desired
velocity state is also specified as a constraint (Figure 4.14). The analysis is done along
the same lines as for the autonomous case in the previous section whereby the common
action of the acceleration and velocity screw is determined. In addition to determining
the joint angular velocities by using Equation (4.13)
'»'=^^'
Af
(4.30)
the joint angular accelerations are generated from (Equation (4.14):
68
0Ato + ^t)
Desired Path
Figure 4. 14: 3R Manipulator with pose and desired velocity
a. =
At
0)^
6i
(4.31)
In a way analogous to the pervious section, in order to determine the errors in the
desired and calculated accelerations, the desired acceleration state is determined from the
velocity state of the manipulator (again, the assumption being that the velocity function is
continuous and differentiable). From the desired acceleration state, the joint angular
accelerations are determined as follows.
The equation determining the reduced acceleration state of a manipulator is:
Ar =
,<y,°$'+ ,ft),'$^ + 2<y3^$^ +
0"'l "J f i"'2
Ocl , Ic2 , „ 2(ij3l
0)^ $ +2^3
,«/2
+
(4.32)
.0^1 $
where <y 's represent the joint velocities, the cb's represent the unknown joint
accelerations, and $ are the screws that comprise the Jacobian. Once the reduced
acceleration state is known, the joint angular accelerations are determined:
69
The above equation can be written as
which in turn can be written as:
[J] Q} + [L] = A^ (4,34)
[J]d) = A„[L] (435)
and finally solving for the acceleration,
^ = [Jr[A,[L]] (4.36)
where [L] is the term containing all the Lie products.
In this case, the path tracking of a 3R manipulator was simulated with the desired
velocity added as an additional constraint. The algorithm generates the command
velocities and accelerations by using the position and desired velocity information. For
comparison, the algorithm calculates desired acceleration state (from the desired velocity)
and determines the desired joint accelerations. These are compared to the command
velocities and accelerations to determine how closely they follow. The algorithm
flowchart is shown in Figure 4.15.
As in the previous case, the link lengths of the manipulator are set as unity. Two
different cases were considered. In the first case (Case I), the path to be tracked was the
same as the one considered for the poseonly case (main54.m, Appendix B). This was
done so that the results could be compared. The path simulation is shown in Figure 4.16.
Figures 4.17 through 4.20 show the angular velocities and accelerations and the errors. A
second case (Case II) was considered (mainSa.m, Appendix B) in which the path was
kept the same but the orientation of the end effector was varied as it traversed the path.
This case is illustrated in Figures 4.21 through 4.25.
70
Given the continuous path
(desired position and
orientation) and the desired
velocity state.
Perform inverse position
analysis
at current & desired pose
6's: current
and desired
are known
Determine the command joint
angular velocities and
accelerations that satisfy
criteria at the pose and velocity
level
Determine the joint angular
velocities and accelerations
using the velocity state, and the
acceleration state
Compare results.
The difference between terms is
the control error.
Figure 4. 15: Algorithm summary for 3R Manipulator path tracking using pose
and desired velocity information.
71
Tracking of 3R manip. using inv. analysis (Pose, Velocity)
Figure 4.16: 3R Manipulator path tracking, using pose & desired velocity
information. Motion of the manipulator and the traversed path. Case I.
2
8
Actual and Desired Angular Velocities
•'/.AI^T?
tinne
omegaOl
omegaOlj
omega 12
omega12j
omega23
onriega23^
Figure 4. 1 7: 3R Manipulator path tracking with pose and velocity information.
Actual and desired angular velocities. Case I.
72
6?
■\ q" Errors in Joint Angular Velocities
o
1 1 1 1 1
1
— error wOl
 enorw12
5
enor w23
.
'
4
■
3
' ^ .
2
■ ,■
1
,
A _
■'I
1 1 1 1 1 1
1 2 3 4 5 6 ■
■
time
Figure 4.18: 3R Manipulator path tracking with pose and velocity information.
Errors in joint angular velocities. Case I.
'1'^n
Actual and Desired Angular Accelerations
I3U
1 1 1 1 1
1
— alphaOl
alphaOl^
100
50
i / '  
alpha12
alpha12j
alpha23
alpha23^
1

C
1 r.^,l i
•.
50
i. ;^
100
!
■150
1 1 1 1 1 1
12 3 4 5 6 7
time
Figure 4.19: 3R Manipulator path tracking with pose and velocity information.
Actual and desired angular accelerations. Case I.
80
60
40
20
20
40
60
80
73
Errors in Joint Angular Accelerations
enor alphaOl
enoralpha12
error alptia23
3 4
time
Figure 4.20: 3R Manipulator path tracking with pose and velocity information.
Errors in joint angular accelerations. Case I.
Tracking of 3R manlp. using inv. analysis (pose, vel.)
0.8
0.6
0.4
0.2
0
0.2
0.4
1 1
1 1
'41 '
1 1
— desired path
\/ y i
f
^#MX
jM
^^^W
1
V ■
\\
'm^^k
m
1
.
'iwyY^A/m
K\\\\
i ■
 mX/vW
^xM
^^
^^*t=fc=~v
IiXXaA •%' Y'f~
__p^± ~^\
'ii
\\^^r\\\v^ — ~*^^
^^^=r \
^^^^
^
in^^^^^^^— 7
+^^^
==^
^s^
^^^=^^^^==
1 1
1
1 1
1 1
0.8 0.6 0.4 0.2
0.2
0.4 0.6
0.8
Figure 4.2 1 : 3R Manipulator path tracking, using pose & desired velocity
information. Motionof the manipulator and the traversed path. Case II.
4
6
74
Actual and Desired Angular Velocities
QA
1.5
time
— omegaOl
omegaOlj
omega 12
__ omega 12j
— omega23
omega23^
Z5
Figure 4.22: 3R Manipulator path tracking with pose and velocity information.
Actual and desired angular velocities. Case II.
25
x10""
Errors in Joint Angular Velocities
enor
wOl

enor
w12

enor
w23
2
1.5

' * :

1

j
'm
0.5

; ■"?

: : \ ■
r ■
^, ■ ' \/
0.5
.1
"
— .1 1 1 1

0.5
1.5
time
2.5
Figure 4.23: 3R Manipulator path tracking with pose and velocity information.
Errors in joint angular velocities. Case II.
75
1'^n
Actual and Desired Angular Accelerations
ov
1 1 1 1
alphaOl
alphaOl^
100
50
50
— alpha12
alpha12j
alpha23
alpha23^
■\
1 ■' 
100
1 en
1111
■ 0.5 1 1.5 2 2.5
time
Figure 4.24: 3R Manipulator path tracking with pose and velocity information.
Actual and desired angular accelerations. Case II.
fin
Errors in Joint Angular Accelerations
0\f
60
40
1 t 1
'
— enor alphad
— eaor alpha 12
enor alpha23
''■•
20
20
■"",'. _ ■'"'■'"■ " ■
'■] ■" ■
40
■■• ■
60
80
1 1 1 1
0.5 1 1.5 2 2.5
time
Figure 4.25: 3R Manipulator path tracking with pose and velocity information.
Errors in joint angular accelerations. Case II.
' 76
Parallel Manipulators
Parallel mechanism based approaches to manipulator design have grown in
popularity in the past decade or so. Mechanisms for a variety of applications including
robotics, machine tools, measurement machines, and sensors have been developed based
on parallel devices.
A parallel mechanism or a platform is defined as any mechanical device that has
legs (connectors) that connect a moving platform to a base. This type of mechanism
possesses desirable characteristics of high accuracy, high payloadtoweight ratio, and
good static stability. The geometrically simplest parallel mechanism has the structure of
an octahedron and it is designated as a "33 platform" since there are three connecting
points on the base and the top platform respectively. This geometry while being simple,
has a serious mechanical disadvantage as it is very difficult to design concentric ball and
socket joints at each of the double connection points on the octahedron without
mechanical interference. To avoid the shortcoming of this platform, platforms with
different geometry have been developed, some of which are the 63 platform [Ste65], the
66 platform (hexapod), and the special 66 platform [Gri93].
To control these mechanisms it is necessary to determine accurate compliance
models. These kinematic models can be obtained if the position and orientation of the
moving platform is known relative to the base. This is known as the inverse kinematic
analysis for the system. The complement of the inverse analysis is the forward analysis,
which entails determining the position of the moving platform with respect to the base
given the six connector lengths. For a general 66 platform, the closed form forward
analysis equation is a 40'*' degree polynomial [Rag93], which is computationally
impractical for real time control. On the other hand, the forward solution of a 33
77
platform involves solving an eighth degree polynomial in a single variable. The special
66 platform provides combines the benefits of both of these platforms. These platforms
allow for the simple analysis of the 33 with an eighth degree polynomial, and allow for
the general benefits of the 66 by eliminating mechanical interference.
Based on the combination of these advantages, the special 66 platform has been a
subject of research at CIMAR for a variety of applications; among them is the
development of a forcefeedback endeffector, as well as the study of the platform as
possible machine tool for milling applications [AbbOO]. Currently, the platform is being
used as the basis for development of a Weight Compensating Parallel Manipulator
(spatial jack) to handle large payloads.
Tracking Methodology
The path tracking methodology developed for parallel manipulators involves the
use of the inverse position and the inverse velocity analysis. The length and the velocity
of each connector are then determined.
The initial attempt to solve this problem was by assuming an acceleration profile
that was linear, i.e. the variation in the acceleration was linear (Appendix A). The results
obtained from this analysis indicated that this assumption would not yield a result that
would satisfy the position, velocity, and acceleration specifications at the end of each
time step. To overcome this problem a different approach was attempted which is
discussed below.
To meet the path tracking criteria, an approximation was required that would
produce the desired motion performance at the beginning and end of each step. Since the
criteria for motion planning were assumed to be position, velocity, and acceleration, these
criteria involve six conditions at the beginning and end of each step. These conditions
78
are the initial and final position, velocity, and acceleration of the actuators, respectively.
Thus the approximation is based on a fifth order polynomial that has 6 constants. The 6
constants match the number of conditions required to satis
fy exactly the position,
velocity, and acceleration state at the beginning and end of a time step.
Consider the polynomial
S(t) = Qq + ^1^ + ^2^^ + ^3^^ + '^4^^ + ^5^'
(4.37)
differentiating with respect to time
5(0 = a, + 2a2t + Sflj/^ + 4a^t^ + Sa^t"*
(4.38)
and again
s'{t) = 2^2 + 6a^t + lla^t^ + 10a/ .
(4.39)
For / = , and
5(0) = 5o
siO)^s,
s'(0) = sl
subsfituting in Equations (4.37),(4.38),and (4.39), yields
^0=^0 . '.y
(4.40)
s,=a,
(4.41)
sl2a2 or
(4.42)
' 2
(4.43)
Substituting the known coefficients above in Equations
(4.37),(4.38),and (4.39),
we obtain
s(t) = So+ SqI + ^sy + a/ + a^t'^ + a/
(4.44)
s\t) = 5o + sit + 3a/ + 4a/ + 5a/
(4.45)
79
s\t) = sl+6ajt + \2a^t^ +20a^t^. (4.46)
At the end of the time step At , the first, second and third order equations become
Sj =Sq+ SqAI + jslAt^ + a^At^ + a^At* + a^At^
Sf^SQ^slAt + 2>a^At^+Aa^At^+5a^Ai*
s'j sl+6a^At + \la^At^ +2Qa^At^ .
Rewriting the equations above and substituting d for At for the sake of clarity,
s^Sq+ s\6 + ^s'qS^ + a^S^ + a^5'^ + a^5^ (4.47)
j^ = 5o + 5o^ + 3<33^^ + 4a4^^ + Sflj^" ' (4.48)
s'j^=sl+6a^5 + \2a^S^ +20a^S^ . (4 49)
The equations above have three unknowns, a^, a^, and a^ . These equations were solved
using Maple®, and yielded the constants as
hsl5'' + 205,  205„ \2s'8 + s\5^  %Sr5
' 2
>
V
Qiy r jj^L
'r
a, =
Gs'qS^ +I5sf 15sq SsqS + s'j^5^ Is'^S
1 f5sl5' + 125 .  125o  6slS + s\5^  6s' 5^
(4.50)
(4.51)
(4.52)
Once all the coefficients {a^, 02,0^,0^,0^) are known. Equations (4.37), (4.38),
and (4.39) can be used for generating commands that satisfy the desired conditions for
position, velocity, and acceleration.
66 Parallel Manipulator
The parallel mechanism considered as an example is the special 66 parallel
platform. The reason for considering this platform is the availability of a design
methodology for designing such a platform to suit a particular application. The
80
parametric methodology developed by Abbasi et al. [AbbOO] is used and is concisely
presented here.
Generally, when designing a platform, the anticipated application must be
considered. The application defines the loading conditions, workspace geometry, and
dexterity, based on which the platform parameters are optimized. The number of design
variables for the special 66 platform is many. To examine various designs in a proficient
manner a methodology has been developed that reduces the design variables.
The platform kinematics can be
described parametrically by defining two
triangles, the base and the top, and six side
pivot connections. An equilateral triangle is
used as provides symmetric stiffness and
response over the workspace. The
equilateral triangle can be defined by one
parameter, the side of the triangle. The top
triangle is related to the base triangle by the
parameter tsidescale. The parameters
bscale and tscale define the base and top
side pivot points, respectively. Another
parameter, hscale, is used to define the
perpendicular distance between the top and
the base of the platform. This parameter is
also a function of SIDE. If hscale is equal ^. ^ ^^ r^ ■ . • .
Figure 4.26: Design parametenzation
of the special 66 platform.
81
to 1, the distance between the top and the base of the platform is equal to SIDE, and a
variation of this parameter would vary the height of the platform accordingly.
The special 66 parallel mechanism requires that the pivot connection lie on the
line that contains the two vertex connections. For example, in Figure 4.26, side pivot
connection 2 must lie on the same line that connects 1 and 3. On the base, if bscale is
less than 1, then the side pivot lies between the two vertex connections and vice versa.
The same is valid for tscale with respect to the top.
A given platform may have a large kinematic workspace, but its stability over the
workspace may vary enough to have affect on its performance. The parameter SIDE
allows the mechanism to be scaled up or down. The other parameters define the
kinematic workspace and variation of static stability across the workspace. This
methodology allows finding suitable parameters that define a platform that is well
behaved over the workspace.
The inverse displacement analysis is useful in developing the expressions for the
dynamic state of the system. The platform location must be specified for a particular
task. The displacement and orientation of each of the connectors that actuate the
platform can then be determined once the platform's motion time history has been
specified. From the motion specification, the velocity and the acceleration state of the
platform is computed and subsequently, the actuator speeds and accelerations are
determined. The analysis is further extended to determine the forces in the actuators.
Algorithm Implementation
The objective of the algorithm is to provide coefficients for the command
equations (4.37)(4.39)developed two sections ago. Another objective of the algorithms
is to provide feedback in terms of the leg velocities and accelerations, and leg forces, to
82 ,
enable the designer to check the viability of the commands; i.e., by getting the feedback it
can be determined whether the hardware would support and be able to match the
commands (generally, a velocity command with limits on velocity and acceleration).
Initially, an algorithm (Movplatlc.m, Appendix B) was implemented, which
when given a desired pose, velocity, and acceleration, would compute the position,
velocity, and acceleration of the connectors using the inverse analysis (reverse66.m.
Appendix B) and subsequently would generate the coefficients for the command
equations to meet the position, velocity, and acceleration conditions.
For this case, the platform design parameters were taken as follows:
SIDE = 30 inches
bsacle = O.S
tsidescale= 1.0
tecfl/e = 0.8
I
,,..■''■
hscale = 1 .0
and the desired motion was a pure rotational motion defined by the motion of the center
point of the platform about the zaxis (vertical). The rotation was described by the
Leg Displacements of the Ptatform
Figure 4.27: Leg Displacements for the 66 platform,
executing a pure rotation.
83
following rotation matrix:
R =
cos(5A0 sin(5A0 O'
sin(5A/) cos(5A0
1
The algorithm computed the six leg displacements, velocities, and accelerations.
Based on this information, the coefficients were calculated for each leg from Equations
(4.40), (4.41), (4.43), (4.50), (4.51), and (4.52). Figures 4.27 through 4.29 show the leg
displacements, leg velocities, and leg accelerations of the manipulator.
This analysis was further extended to determine the forces in the platform
actuators. Using the concepts of dynamic analysis of rigid bodies using Screw Theory
facilitated this. The equilibrium equation for the platform can be written as
[J]f,ess= W (4.53)
where [j] is the Jacobian composed of the screw coordinates of the platform actuators,
fie& ^r^ the forces in the legs and W is the wrench on the platform.
The wrench on a rigid body can be determined as
W ^ I A^ + [V IV]
where / is a 6 x 6 inertial matrix and is composed as follows
(4.54)
'6x6
MR MI
I
'3x3
d
MR
(4.55)
MR is a coordinate transformation for different reference frames, and for this particular
case, is a 3x3 matrix of zeroes. MIj is the mass matrix times the identity matrix which
produces a diagonal mass matrix. And finally, the 73^3 is the inertial matrix given as
84
Leg Velocities of the Platform
0.5
0.5
1 1 1 r
1 —
 LegKD
^
""^
Leg2(q)
Leg3(m)
Leg4(r)
y^
'.'
Leg5(n)
/ V ^
V V V
Leg6(p)
Vj
/
y V ^
^
/ V
/

/ ■• . . V
/
/ .'< V
/
/ ^ ., ' ; .■
i /
/ V •' ■ ■ ■ ■ '
/
/
y< V
/
/ V
^ /
/
/ V' '^ '^' ^^■ .
/
/
1
/ V "^
/
X V
/
x^ ^'^ ^ '
/
^ ^ /
/
V f^ f
/
—
/
^5K , " /
o<   • . /
V ^ ~^^ £. A £i ^ ,,' ^
^ ^ ^
^ x'
^^ v"
1 1 1 I
1
0.1
0.2
0,3
time
0.4
0.5
0.6
Figure 4.28: Leg velocities for the 66 platform executing a pure rotation.
Leg Accelerations of the Platform
I'
V V V V
Leg1(l)
" Leg2(q)
Leg3(m)
Leg4(r)
V Leg5(n)
Leg6(p)
0.1
0.2
0.3
time
0.4
0.5
0.6
Figure 4.29: Leg accelerations for the 66 platform executing a pure rotation.
85
'3x3
I^
I.
I J
I.
iyy
J.
I.
I.
^J
(4.56)
Further, A^ is the reduced acceleration state and V is the velocity state of the top
platform and are determined as
A, =
a
(4.57)
V =
0)
Un
(4.58)
And finally, the term in the square brackets, [V IV], is the Lie product of two screws
and is determined as shown in Equation 2.55. Once all the components are known, the
forces in the actuators are calculated from (4.53) as
//...= W^ (4.59)
This procedure was implemented using data for a platform being developed at
CIMAR as a force compensating manipulator device. The mass and inertial matrices
were formed from this data. The mass matrix was found to be
MI, =
79.5928
79.5928
79.5928
kg.
(4.60)
and the inertial matrix was calculated as
'3x3
3.39146
3.39146
6.59053
kg.m
(4.61)
86
The function that calculated the inverse position, velocity, and acceleration for the
previous case, was further developed to include the force analysis (reverse66f.m,
Appendix B). This algorithm was tested for a variety of cases, such as constant velocity
motion (translation), translation with constant acceleration, rotation with constant angular
acceleration, etc. The results from two cases considered are presented:
• In this case, a constant acceleration motion was implemented as the platform
moved in the vertical direction; results are shown in Figures 4.30 through 4.33
(Movplat2b.m, Appendix B).
• A rotation of the top platform about the center (zaxis) with no translation was
implemented, with constant angular acceleration; results obtained from this
simulation are presented in Figures 4.34 through 4.37 (Movplat2d.m, Appendix
B).
Techniques were developed in this chapter for generating command screws that
meet the desired position and velocity specifications for autonomous robots, serial
manipulators, and parallel manipulators. The following chapter presents the case of a
serial manipulator with a control system implementation.
87
Leg Displacements of the Platform
0.95
0.9
0.85
E 0.8
0.75
0.7
0.65
0.8
1
 LegKD
Leg2(q)
Leg3(m)
Leg4(r) "
Leg5(n)
Leg6(p)
 ^^_v_„_^^^^v^^?V'Vv^?v^:?v^^"^

1 1 1 1 1

0.1
0.2
0.3
time
0.4
0.5
0.6
Figure 4.30: Leg displacements for a platform motion with translation and constant
acceleration.
0.06
0.05
0.04
I 0.03
0.02
0.01
Leg Velocities of ttie Platfomfi
1 1 1 1
r
LegKD
^
Leg2(q)
.■.■'■'.■ a'
Leg3(m)
Leg4(r)
V
Leg5(n) 
A
Leg6(p)
A
■ 'X A7
A '  V
A ■ X .

.X
A J/ y'
ii ^ ^'^
/■ ^'^^"

■^ ^ ^"
A y X
A .^ ^
.■• J^ ■'
A J^^y . : ■
A .J/^"
. ^^''
.^r^" ■
_
X^
'■ V?"^
• x^^ ■
'^
— ' 1 \ 1 1 1
0.1
0.2
0.3
time
0.4
0.5
0.6
Figure 4.3 1 : Leg velocities for a platform motion with translation and constant
acceleration.
88
0.12
0.115
11
Leg Accelerations of ttie Platform
 ^...^..^..AAAA"A"AAAAA.A...AA"A'.a'A"'£''^^^ '^^
Legl(l)
— Leg2(q)
Leg3(m) .
 Leg4(r)
V Leg5(n)
Leg6(p)
0.105
1 °'
0.095
 ■   .
0.09
_ ■
0.085
. ^^_^,^.^^^wv^.^vv^^^^""^^'^ ^
0.08
0.075 ' ' ' ' ' ' '
0.1 0.2 0.3 0.4 0.5 0.6
Figure 4.32: Leg accelerations for a platform motion with translation and constant
acceleration.
a
Leg Forces of the Platform
6
4
. Si a. £i, £i & £^ a. d £i £i a ^ ^ j^ a £i t.
Legl(l)
 Leg2(q)
Leg3(m)
 Leg4(r)
V Leg5(n) 
Leg6(p)
V
'vvvvvvvvvvvvvvvvvvvvvvvv
z 2


2
.4
1 1 1 1 1
C
t 0.1 0.2 0.3 0.4 0.5
time
6
Figure 4.33: Leg forces for a platform motion with translation and constant
acceleration.
89
0.9
Leg Displacements of the Platform
1
LegKD
—
Leg2(q)
Leg3(m)
0.85
~ y
Leg4(r) "
V
Leg5(n)
/ V
Leg6(p)
0.8
y V

.,....■■. ^.iV ^ : V
■ ' ♦ . s ■'"  X
■' ■ > ^ '...i . \ '" " X V
0.75
E 0.7
0.65
^ ~ . V ..■■■■ '
 ^ ^ V
>7
0.6
V ^ ^ ^>■••■■■■
^^^?~ll ■■■■••■■■■■■' ^^.
■
0.55
1 1 1 1 1

0.1
0.2
0.3
time
0.4
0.5
0.6
Figure 4.34: Leg displacements for a platform motion with rotation about the vertical
axis, no translation, and constant angular acceleration.
1
Leg Velocities of ttie Platfomn
Legl(l)
^
""
Leg2(q)
Leg3(m)
y^ ■
Leg4(r)
y^
Leg5(n)
Leg6(p)
X V ^ ^ ^ ^ v>
1
0.5
/ V /
/ ^ ^ /
/ . ^ "^ '' /
 ^ A ,^ /
V ^  ^
^^ y
'^. /»
"■ .. >■''''
n»i
1 1 1 1 1
0.1
0.2
0.3
time
0.4
0.5
0.6
Figure 4.35: Leg velocities for a platform motion with rotation about the vertical axis,
no translation, and constant angular acceleration.
90
Leg Accelerations of the Platform
8
LegKD
^

Leg2(q)
1  /
Leg3(m)
/
Leg4(r)
6
/
V
Leg5(n) 
/
Leg6(p)
/ /ii ^
■ ■  , / £1,'
/ /
/ "/
4
/ ^ /
O
7"^>^.
i! 2
?
. '  V

A
1 1 1 1 1
0.1
0.2
0.3
time
0.4
0.5
0.6
Figure 4.36: Leg accelerations for a platform motion with rotation about the vertical
axis, no translation, and constant angular acceleration.
200
150
100
50
Z
Leg Forces of ttie Platform
50
100
150
200
1 1 1 I y ■■
1
V
Legl(l)
Leg2(q)
Leg3(m)
Leg4(r)
Leg5(n)
Leg6(p)
1

V V V V 'V' V V V V V V 
V,
\ '
\
\
1 1 1 1
0.1
oz
0.3
time
0.4
0.5
0.6
Figure 4.37: Leg forces for a platform motion with rotation about the vertical axis, no
translation, and constant angular acceleration.
CHAPTER 5
PATH TRACKING AND CONTROL
This section discusses the control of a manipulator by developing a system model
and applying the path tracking techniques developed in Chapter 4.
Manipulator Model
A manipulator model was developed for testing the path tracking algorithms
developed and to study the system response. The model was based on a three link planar
manipulator with all three joints being revolute. The link lengths were selected which
defined the geometry of the manipulator as well as the motor torque requirements. It was
decided to use electric motors as direct current (DC) motors lends themselves well to
speed control applications. The torque requirements were calculated based on the
mechanism geometry and performance desired. The mechanism parameters are (Figure
5.1):
/i = 30 cm, h = 30 cm, and /i = 15 cm, with a payload of 2.5 kgs.
Dynamic equations of
the motor are used to
determine the motor transfer
function. The transfer
function is then used to model
the system parameters. The
Figure 5.1: A 3R Planar Manipulator.
91
92
electrical equation of a motor is given as [Rel]:
V = L^^ + RI,+K,co (5.1)
where j^ ; ., , a^ •. ' ^  . '
V = motor voltage
'*>
L = motor inductance
a
/^ = motor current
/?= motor resistance
AT^ = electrical constant, and
(o = motor velocity.
The magnetic field in the motor is constant and thus the current produces a proportional
torque (Tg),
The opposing torque of the motor (r„) is a function of the constant friction torque (T^),
and the viscous friction proportional to the velocity ( Deo ), and can be written as
r„ = T^+Dco. (5.3)
The relationship between the torques and velocity is the following:
Tg = {J„+J,)^ + DcD + T^+T, (54)
where J^ and J, are the moments of inertia of the motor and the load respectively, and
7] is the load opposing torque. Equation (5.4) is the dynamic equation of the motor, and
along with Equations (5.1) and (5.2), it describes the relations between the electrical and
the mechanical variables.
93
When a motor is used as a component in a system, the transfer function of the motor is
required which can be determined from the equations described above. Assuming T, and
Tj equal to zero, since neither effect the motor transfer function. Equations (5.1), (5.2),
and (5.4) can be written in the Laplace domain as
V{s) = {sL^+R)l^is) + KMs) (5.5)
T,(s) = iJ„+J,)scois) + Dcois). (5 7)
Representing the total moment of inertia as J , Equations (5.6) and (5.7) can be combined
to derive an expression for the current which when substituted in Equation (5.5) yields
V{s) = ^{sL^+R){sJ + D)cois) + K,o>(s) (5 g^
and the transfer function of the motor is
V{s)
K, (59)
{sL^+R){sJ + D)+K^K/
The transfer function has two poles, which in all practical cases are negative and real.
The transfer function can be written as
{ST,+\){sT^+\)
K, \ (5.10)
JL^ (sPi)isP2)
where r, and Tj are the time constants and p^ , p^ are the poles of the transfer function.
The time constants are related to the poles by
94
T, =l/p, and r2=l/p2. (5 11)
Considering the case where the damping factor D is negUgibie (=0) and the inductance
L is small, the transfer function becomes .
G„{s) =
Kr
s^LJ + sRJ + K^Kj.
(5.12)
and the poles are the roots of the characteristic equation
s^LJ + sRJ + K^Kj. = ,
(5.13)
Using the approximation for L^ being small, the poles are found to be
— K. cK.j , — K
and P2 =
RJ '' L
and from Equation (5.1 1) the time constants are
RJ , L^
r„ = and r, = — 2
£ r
(5.14)
(5.15)
where r is the mechanical time constant and r^ is the electrical time constant. The
relationship between the poles and the constants is valid only when the mechanical time
constant is an order of magnitude larger than the electrical time constant, which is true
from most DC brushless servo motors.
Motor Specifications
The torque requirements for the motors were developed based on the manipulator
size and desired performance. Based on the requirements, DC brushless servomotors
were the motors of choice; and specifications are shown in Table 5.1.
Table 5.1 Manipulator specifications.
Link
1
2
3
Link Length
30 cm
30 cm
15 cm
Torque Requirement
4.97 N.m
1.12N.m
0.26 N.m
Motor Selected
ElectroCraft
F4050
ElectroCraft
Y 20121
ElectroCraft
LD 2003
Torque (cont./max.)
5.2/13.6 N.m
1.4/3.8 N.m
.34/1.02 N.m
95
System Model
For the purpose of this analysis, the motors are modeled as a first order system
and stepresponse modeling is carried out. The basis for this assumption is the fact that
overdamped second order and higher order systems can often be approximated by a first
order system with dead time [B0I88]. The stepresponse indicates the form of the process
and the coefficients in the process model can be determined. The input output
relationship (transfer fianction) of a first order system in the Laplace domain is given by
yjs)^ 1
xis) Ts+\ (5^6)
where t is the time constant. Applying a unit step function \/s as input in the equation
above, we get
y(s) = 
s rs+l
and rearranging
yis) =
and subsequently taking the inverse Laplace transform yields
t
yis)^\e'. (5.18)
It should be noted from Equation (5.18) that the smaller the time constant r , the faster
the system response.
For implementing the system model, a discrete process model was used. The
discrete transfer function is based on the use of difference equations. The backward shift
operator B is used, which is defined as
96
By„ = >'„,, B y„^ y„_^, ... , B^y„ = y„_j. (51 9)
The discrete transfer function for a first order system with a time constant is given
by
X.
K(\e"')B
\e"'B '
and the discrete transfer fUnction for a delay process is given by
^ = KB'',
where
T  sample time
T = time constant of the motor
B  backward shift operator, and
K= motor gain
D = delay, and
d = D/T = an integer.
The transfer function for a first order process with a delay then becomes
X.
K{\e^")B''''
\e"'B
and is shown in Figure 5.2.
Figure 5.2: A process with delay.
(5.20)
(5.21)
(5.22)
PROCESS
DELAY
w
W
W
^
97
Controller Development
Once the process (motor) model has been developed, the design of an appropriate
controller was the next step. The objective being to design a controller that will allow a
response that meets the transient and steady state requirements of the closed loop system.
The most widely used controller in industrial control systems, the proportionalintegral
derivative (PID) controller is a combination of the three basic control actions and
provides the advantages of all the three individual control actions. The equation of a
controller with PID action is given by . y
y{t) = K^ e{t) + ^ ]e{t)dt + K^T, ^ (5.23)
where e is the error between the input and the output, K^ is the proportional gain, T. is
the integral time, and Tj is the derivative time. The transfer function of a PID controller
IS
2(£) ^ ^ r. 1 ^
eis)
(5.24)
or alternatively it can be written as
—  ^.+V + ^.^ (5.25)
where K^ is the integral gain and K^ is the derivative gain. In this case, when a
controller is tuned, the gains are the adjustable parameters.
Once the controller type was selected, controller tuning for the process was done.
Controller tuning is a method of selecting the controller parameters to meet the desired
performance specifications. The method employed for tuning was the Ziegler and
Nichols rules for tuning PID controllers. The Ziegler and Nichols rules [Oga97] are
98
based on experimental step responses or the value of K^ that results in marginal stability
when only the proportional control action is used.
The second method of Ziegler and Nichols rules was used for determining the
controller parameters. In this method, the integral time 7] is set to infinity and the
derivative time T^ is set to zero. Using the proportional control action only, the
proportional gain is increased from to a critical value K^^ where the output first
exhibits sustained oscillations. The critical gain K^^ and the corresponding period P„
are experimentally determined. The critical period is the time period for one complete
oscillation of the output with critical proportional gain.
fciO 
PID
^
1
1 1
^S
L
.005753S+1
step
Input
PID
Centre
ller T
ranspo
Delay
rt
Transfer Fen
Response
Figure 5.3: A closed loop system for controller tuning.
The tuning system was set up using the Simulink® software by modeling each
motor in a feedback loop with a controller. The setup for each motor resembled the block
diagram shown in Figure 5.3. The time constant, r , for each motor was calculated using
the data in Table 5.2 and was found to be
Table 5.2: Parameter values for determining the mechanical time constant of motors
Motor
1 (F 4050)
2 (Y 20121)
3 (LD 2003)
R (Ohm)
.54
1.3
3.0
J(kgmO
.0022
.000032
.000013
A:^(V/kRPM)
66
29
16
/^^(Nm/A)
.54
.24
.15
99
r, = 0.005753 sec.
72 = 0.00063 sec.
Tj = 0.00170 sec.
The integral and derivative gain values were set to zero and the proportional gain was
varied until the response exhibited sustained oscillations. This gain was further tweaked
to within a reasonable precision. Figure 5.4(a,b) illustrates this process. Once the critical
gain was obtained, the critical time period was determined, and subsequently, using the
Ziegler Nichols rules, the proportional gain was obtained as
K^=0.6K^^, (5 26)
following which, the integral time 7] , and the derivative time T^ was calculated
Z = 0.5 P
7;= 0.125 P,,
which finally yielded the gains as
K.=—^ and
.5 P„
K,^0.\15P,^K^.
Step mput RMponM «Mi PID gMn«
(5.27)
(5.28)
(5.29)
(5.30)
0.1 0.2 0.3 04 0.3 0.0 0.7 09 0.9 1
0.1 0.2 0.3 0.4 0.5 OJ 07 OJ 0.0 \
Figure 5.4: (a) Sustained oscillations at critical proportional gain (Motor 1). (b)
Response to a step input with the PID controller (Motor 1).
100
It is important to note that the values obtained by this method are essentially an
educated guess which provides a starting point for fine tuning the system, and hence the
systems were fine tuned to obtain a smooth step response. It is, of course, also possible
to add other advanced techniques such as feedforward control, disturbance rejection,
adaptive control, or a combination thereof, for designing an optimal controller, but for the
purpose of illustrating the path tracking in this case, PID control was deemed sufficient.
Controller Implementation
A discrete PID controller was implemented to control the process described in
Equation (5.22). The controller is of the form
m.
where
= w„_, + ^0 er„ + k, er„_, + k^ er„_^ (5 j^)
m„ = controller output
er = (command  actual) error: controller input
ni= values at i timesteps back, and
r^, A:, , ^2 , are constants
computed as
= K+KJ + ^
p > Y
(5.32)
' " T
(5.33)
2 J,
(5.34)
where T is the sample period.
Once the process transfer fiinction and the controller transfer fiinction were
determined, the control loop was setup as shown in Figure 5.5. The discrete controller
101
algorithm was implemented in MATLAB and the results were tested to correspond to the
continuous time controller used in Simulink to model the step response. Once the results
were verified, the control algorithm was amalgamated with the pathtracking algorithm
for a 3R Manipulator.
>^ ^\
PID
CONTROLLER
PROCESS
yn
""".^
""%
DELAY
W
►(* >
\ /
p
i
Figure 5.5: Closed loop PID controller for a process with delay.
The discrete pathtracking controller uses the following equation, based on
Equation (5.22), to generate its output that is dependent on the previous value of the
output and the control effort generated by the controller. Using Equation (5.22) and cross
multiplying, we obtain , , ^
y„ = e"^By„^Kile''^)B''^\.
Finally, applying the backward shift operator
(5.35)
(5.36)
(5.37)
yn = e"'y„_,+K{\e"')x„^,,,.
Writing the equation in terms of control variables we obtain.
(5.38)
^a(n) = e ^ «„(„.„ + /: 1 1  e ^ I m„.,_.
(5.39)
where
102
0)^ = system output (actual angular velocity) at time step n
®a(ni)  actual angular velocity at previous time step
fn„_i_d = control effort at previous time step or earlier depending on the delay.
The controller was implemented for a 3Rmanipulator path tracking using the
methodology developed in Chapter 4. The flow chart for the algorithm is shown in
Figure 5.6a and a block diagram of the process is shown in Figure 5.6b. The algorithm
essentially represents an open loop tracking control of the manipulator. The controller
loop operates at a higher frequency than the tracker command frequency. The option to
adjust the controller loop frequency is a feature in the program. Another variable is the
delay time D (d in Equation (5.39)), which can be fluctuated to study the behavior and
response of the system and evaluate its performance.
Initially the program (main54cPlDlc.m, Appendix B) was tested with a delay of 3
milliseconds (ms). To arrive at a reasonable estimate for a delay time, the following idea
was used: a first order system, in response to a step input, reaches 99.3% of the final
value in 5 time constants. Using this number and the motor with the highest time
constant, and the assumption that the delay is approximately .55% of 5 time constants,
the delay was found to be 1.5ms. For a conservative estimate, the delay used was
doubled to account for any other delays in the computer control of the manipulator.
Figures 5.7 and 5.8 illustrate the actual and command angular velocities for each motor
and the errors between the two. This test was performed with a delay of 3ms.
103
PATH
TRACKER
COMMAND
VELOCITIES
W„
Controller calculates
error (actual
command)
er„
Calculates control
effort: Equation (5.31 )
m„
Outputs Effort to
Process/ Updates
Variables
Controller
Calculate Output of the
system using Equation
5.35
W.
/Generate Output A
A Update time J
W„
PATH TRACKER
\^\
RID CONTROLLER
'"%
PROCESS
^a
+>
DELAY
\
) ^
iL
Figure 5.6: (a) Flowchart illustrating the algorithm for path tracking and control.
(b) Block diagram of the process.
104
Actual and Command w1
0.2
0.4
0.6
(
0.5
1
0.5
x.
W1.
^^
/
12 3 4
Actual and Command w2
*2,
V
12 3 4
Actual and Command w3
0.5
1
0.5
■ /
__ w3.
^^y
2 3
time
0.02
0.01
0.01
5
'
5
10
15
I
0.15
.xlO
ftt
0.05
0.05
En^or In w1
2 3
Error In w2
2 3
Error in w3
2 3
time
Figure 5.7: Actual and command angular velocities and errors
2.
Actual and Command w1
0.19
0.18
\
\
_W1^
— w1.
P — — ^~~~^
0.17
'"'"'^''^^ — ,
0.16
■
xlO
Error in w1
0.02 0.03 0.04 0.05
Actual and Command w2
0.1
,0.08
I
I
0.06
_.JH?^";
^yrrr^'
— ^c
W2.
1
■
0.01 0.02 0.03 0.04 0.05 0.06
Actual and Command w3
0.24
I 0.26
0.28
'
— w3^
 w3.
^^
0.05 0.1 0.15 0.2 0.25
time
15
10
5
0.5
1.5
xlO^
Enor in w2
'
5
•
10
0.5 1
time
1.5
Figure 5.8: Magnified view of the command velocities and errors.
0.5 1
En^or in w3
1.5 2
0.04
'
■
0.02
•
0.02
105
■"■. t\
To determine the accuracy of the end effector of the manipulator, the errors in the
end effector position were calculated. The error was determined by obtaining the
difference between the command and actual positions (using the forward analysis) in both
the X and y coordinates. Figure 5.9 illustrates these errors. Furthermore, the statistical
error analysis yielded the following results:
Table 5.3: Errors in the end effector position.
Error
Mean (cm.)
Standard Deviation (cm.)
xcoordinate of the end effector
0.03786
0.05765
ycoordiante of the end effector
0.02666
0.06854
.X10
Error in x coordinate of end effector
1.5 2 2.5 3 3.5
time
Error in y coordinate of end effector
4.5
1.5 2 2.5 3 3.5 4 4.5
time
Figure 5.9: Errors in the end effector position.
On subsequent tests, the delay was increased step by step until the system became
unstable and exhibited oscillatory response. The onset of oscillation was at 50ms and at
106
higher delay times the system did not converge. Figure 5.10 shows the command and
actual velocities at the 50msdelay level. The onset of oscillation is apparent.
0.3
^0.2
0.1
0.1
0.05
^
Actual and Command w1
I 'I .'if '
iiiiiiiiiU
II lip
W1^
Wl
0.2 0.4
Actual and Command w2
'i/^
_w2^ 
 w2.
'/
•
0.05
0.1
Actual and Command w3
0.22
■
— w3^
0.24
A
_w33
0.26
1 \ «
/I / \
1 — LJ ( \
•
0.28
0.05 0.1
time
0.15
Error in w1
0.5
Error in w3
1
0.04
0.02
^1
\k^
r
0.02
v
0.04
,
.
0.1
0.2
tiine
0.3 0.4
Figure 5.10: Actual and command angular velocities and errors with the Delay set at
50 milliseconds.
It is possible to retune the system to where it can handle the higher delay. The
penalty of doing so is the higher rise time or sluggish response of the system.
Experimentation was done for this purpose and the results showed that for systems tuned
to a 100ms delay, the rise time was approximately 2.3 times that of a system tuned for a
10ms delay. A sample response test performed on motor 2 is shown in Figure 5.10a and
Figure 5.10b.
107
Response to step Input: 10ms delay
0.8
0.4
0.2
■ :■;;?
0.1 0.2 0.3 0.4 0.5 0.6 0.7
time
Figure 5.10(a): Step response for a system with 10ms delay.
Response to step Input: 100ms delay
Figure 5.10(b): Step response for a system with 100ms delay.
The results of the simulation indicate that the controller will perform well within a
range of delay times. If the delay times exceeds a threshold, the controller can be retuned
although this would result in a sluggish response from the manipulator. It is anticipated
that this method will produce similar results when applied to an experimental system.
CHAPTER 6
CONCLUSIONS AND FUTURE WORK
Conclusions
The Theory of Screws provides an elegant and useful method for analysis of
mechanisms. In this research, the concepts of Screw Theory have been employed to
develop path tracking methodologies for autonomous robots, serial robots, and parallel
manipulators.
The method for autonomous robots and serial manipulators involves generating a
command screw based on the constraints specified. The constraints can be at the position
or the velocity level. For position level constraints, a command screw is generated based
on the velocity screw. For velocity level constraints, a command screw is generated
based on the combined effects of the velocity and acceleration screw. For autonomous
robots, the accuracy of the generated command screw is verified by choosing a circular
path and plotting the poles of the command screw. For serial robots, commands are
generated using the reverse analysis, and the generated commands are compared to either
the desired velocity state or the desired velocity and acceleration state depending on the
constraints.
Tracking for parallel manipulators is accomplished by introducing a polynomial
that satisfies conditions for each time step. The conditions were assumed to be the
position, velocity, and acceleration at each time step. A fifth order polynomial is selected
that has six constants which satisfy the position, velocity, and acceleration conditions at
108
109
the beginning and end of each time step. The methodology involves the reverse analysis
to generate the displacements, velocities, and accelerations, of the platform cormectors
based on the desired motion. Using this information the constants of the polynomial are
determined which is used as the command for position, velocity, and acceleration.
The analysis for the parallel platforms was further extended to include the inverse
dynamics to determine the forces in the legs. This was considered a beneficial tool for
implementing a tracking and control strategy for a parallel manipulator as the output is an
indicator of the hardware performance requirements of the system.
Path tracking with control was implemented on a 3R serial manipulator. This was
done to study the performance of the path tracking algorithm with the dynamics of the
system. A serial manipulator was modeled, which used electrical motors for actuation.
The motors were modeled as a first order system with a delay. A PID controller was
developed for motor control and was tuned for each motor. The results indicated that the
controller followed the desired tracker commands within a band for the delay.
In summary, a methodology has been established that employs the Theory of
Screws. It is by no means comprehensive but lays the foundation for Screw Theory
based path tracking and control for general categories of robots and manipulators.
Future Work
The method developed for the autonomous robot does not consider vehicular
constraints. The method can be extended to address constraints that are particular to an
autonomous vehicle, such as vehicle with front wheel steering or vehicles with all wheel
steering.
110
The methods do not consider constraints on velocities or accelerations. These
limits can be incorporated into the algorithm when a specific application is known and
the system limitations are known. The algorithm can then generate commands based on
the best achievable value.
Another interesting prospect is the application of the parallel platform
methodology to the spatial jack being developed at CIMAR. It is expected that this
method will provide the basis for the tracking control of the platform. It is also expected
that by being implemented on the manipulator, the algorithm will evolve to a better and
more useful tool.
APPENDIX A
INITIAL PATH TRACKING APPROACHES
f *;
112
Initial path tracking approaches are presented here, the results of which led
to the development of path tracking techniques discussed in the main text.
113
Considering the Pose and Velocity Information '
In this case, the velocity state of the rigid body B is specified, in addition to the
pose (position and orientation) information that is available. This is illustrated in Figure
A.I.
PATH .
Desired pose at time (t,,)
Desired pose at time (to+dt)
Yc
Oc(to) Xc
Current pose at time
(t.)
^o**^
\ X
Figure 4. 1 : A rigid body following a path with a prescribed pose and velocity.
In the most general case, the problem can be stated as: given the current pose of a
rigid body B at time to, together with its velocity state at the same time to, and the desired
timed path of the body, together with the desired time dependent specifications of the
velocity state, develop a tracking technique so that at time to+At, the desired and the
current poses and velocity states coincide, respectively.
The problem of computing rotations about arbitrary axes is quite complicated. At
the outset, it was attempted to solve this problem with a simplified analysis involving
rotations around a fixed axis combined with translations. This assumption is justified as
to this point, as the initial analysis cases are twodimensional. Once a technique is
devised, it will be attempted to devise methods to include arbitrary spatial rotations.
114
Rotation around a fixed axis
In this case, the problem becomes as shown in Figure A.2. Given the current
position 9c at time to and the
0c(to),COc(to)
current velocity coe at time to, it is
required that time to+At, both the
desired and current position and
the desired and current velocity
state coincide.
Assuming constant angular
acceleration a, and integrating the
angular speed equation
codOldt, can be written in difference form as:
0)^ {Iq + At) = 0)^ (to ) + aAt
and integrating again,
e^ {t, + At) = G^ (t, ) + 0)^ (t, )At + X aA/ '
ed(to),o)ci(to)
ed(to+At),cOd(to+At)
Figure A.2: Rotation about a fixed axis.
(A.1)
(A.2)
In general, it will be impossible to simultaneously solve (A.l) and (A.2) with only
one variable, a, available. To overcome this problem, the following concept is
introduced. Two acceleration states will be used, i.e., ai and a2, such that
a(t) =
\a,
to < t < (/o+f)
(A.3)
l«2 (^+f) < i < (^o+AOj
and is illustrated in Figure A.3. Two equations with two unknowns are now solvable.
Integrating the equations as done previously, with a different time interval.
115
6^c(^o+f) = ^cOo) + «iY (A.4)
G),Oo+^) = <»c(^0+f) + «2
A/
substituting, the former into the latter
equation results in:
6>c(^0+^) = ^c(^o) +
— ! ^A/
and finally,
0),{t,) + ^'\^^ M^O},{t^+^.) (A.5)
Similarly, for the orientation we
have:
i
^ a(t)
a
to
i
ai
' a(t)
t
a2
^r
.
to
t
Figure A.3: Acceleration profile.
A/ 1
^c(^o+f)= ^c(^o) + ^c(fo)^ + T«
^A/V
2 2
A/ 1 TA/^
^,Oo+A/)= ^,(/o+f) + «c(^o+f)Y+2"
V ^ y
(A.6)
and as done previously, substituting the former into the latter:
A/ 1 TA/^
^c(^o+^)= ^cOo) + ^cOo)y+2«
Ar 1 Ta/^
V ^ y
+ «c(^o+f)y+2«
\ ^ J
substituting the value for fi;^(/o +y) from Equation A.5
^c(^o+^)= ^c(^o) + «cOo)y+2«i
A/
V ^ y
A/ 1
— + — a.
2 2 ^
^A/^'
2;
^,(/o+A/)= ^,Oo) + «cOo)Af + «i
^A/^
+ —a.
\^ J
At
116
finally results in
3 ^4(Y,i„r4
2 'I 2 J 2 H 2 J
(A.7)
Equations (A.5) and (A.7) lead to the following system of linear equations:
2
a, + oTj =
3«, + a2
At
8
A^
(A.8)
Since the coefficient matrix of the above system is nonsingular,
1 1
3 1
13 = 2^0,
the system always has a solution. The solution is unique assuming the time interval is
halved; solutions can be obtained for other fractions of the time interval.
Translation in Space
The problem statement in this case is, given the current position vector Fc and
velocity vector Vc at time to, as
well as the desired position
vector Fd and the velocity vector
Vd find the motion states so that
Z=X^ i
i.
Vd(U^
r V Vd(to+At)
rd(to)
^^^'"^^^i"
^
'♦.,
rd(to+At) ^
/
r.(t„)
Vc(to) y=x^
x=x'
at time to+At, both the desired
and current conditions coincide.
This is shown in Figure A.4.
Since the solution is the
same for each of the translation Figure A.4: Translation in space,
components, instead of employing the Cartesian x, y, z axes, the axes x', x^, x^ will be
117
used. Furthermore, the problem is totally analogous to the rotation around the fixed axis
problem, and thus the linear acceleration specification is given as:
a'(0 =
a,
L«2
to<t<{to+T) 1
And following the same procedure as in the prior section in the rotation case, the
following equations are obtained:
y'M + ^^^^(y'Ato + ^)
3
^At^'
\^ J
AM
2 H 2
(A.9)
(A. 10)
Thus, for any /= 1,2,3, the linear system becomes,
a;+fl2 = — k(fo+^)<(^o)]
3a,' +a\ =
Af
kit,+A,)r:{t,)vUtoM
(A. 11)
Amalgam of the Translation and Rotation Analyses
The two previous analyses can be combined to provide a solution for the path
tracking problem of a serial, nonredundant, planar manipulator. Combining the results
of these analyses, the velocity and reduced acceleration states for the endeffector of the
planar robot can be obtained. Considering the poses and the velocity states of the end
effector, the velocity and acceleration specifications' for the endeffector at time t=to are:
The index in the formulas goes from to 3 because a 3 link manipulator is considered.
118
'V\(o) = Ki^o) =
'A,\t,) =
Or 3,
0,t 3
O.r 3,
5o, (^o)X(^o)x>oOo)
(A.12)
(A.13)
where d^ (t^) and Oo, (/g) are obtained from the analyses done in previous sections.
The joint velocities and accelerations can be obtained from the following set of
equations [Ric99]: : U \*,^ '' / '* ' * I
'V'ito) = oMt,)'$\t,)+ ,co,it,)'$\t,)+ ,(0,it,)'$'it,)
'A,\t,) = \
d)M°$\t,)+ ,cb,it,)'$'{t,)+ ,(b,{t,)'$\t,) +
coM'$\t,) ,co,(t,)'$\t,)+ ,co,{t,)'$'{t,)\
+
(A. 14)
(A.15)
where the quantities in [ ] brackets represent the Lie product of two screws.
For time t=(/o + y) , the velocity and reduced acceleration expressions become:
0t73
V\t,+f) =
At
Or. 3
0 3,
V'o Oo)+ ^01 (^o)
(A. 16)
'A,\t,+f) =
Vc^o)
«^/(/o)+<'aA/o)f
Vo^(/oKX^(^o)f
(A. 17)
The new joint velocities and accelerations are obtained from Equation (A. 16) and
Equation (A. 17) evaluated at t=(/o +f) as follows:
119
0t73
V\L+f)
''A,\t,+f) = \
+ 2^3('o+f)'$'(^o+f) +
(A.18)
(A.19)
It must be pointed out that during the motion from t=to to t=to+At, there will be
changes in the location of the screws which would yield a result that would not be exact,
and as a consequence of which, there would be a small error between the desired and
required poses and velocity states. However, it can be hypothesized that with the
selection of a small interval At, the effect of this error can be minimized.
Path Tracking Assuming a Linearly Varying Acceleration Profile
Consider the initial and final acceleration after a time step At :
ao=a(Oat/o . ,
Oj^ = a{t) at tg+At .
Now assuming that the variation is linear, it can be written that
ait) = flo +
At
fl(/)«ao+— (a.flo)
At
(A.20)
(A.21)
then
For t=0, u{t)  Vq , thus
^iO = aot + —{afa,)+q.
u(t)^u,+a,t + —(afao).
(A.22)
(A.23)
Integrating again,
120
aJ^ t^
sit) = Oot + ^ + —{a J. a,)+ Q .
2 6A;
Again, for t=0, 5(0 = Sq , thus
(A.24)
aJ t
The values of a . and a^ must satisfy the conditions
(A.25)
therefore
and
s{i) = Sj at /() + At and
u{t) = Uj^ at t^+At
Or=OQ+ a^At + TT(a/  ^0 )
2A/
aAt^ At^
^(,At Ar / \
s{t) = s,+v,At + ^:^ + —[a^a,).
Rearranging the equations,
(A.26)
(A.27)
Vj Uq At
V 2 2,
f
Sf Sq= UoA/ + Ar
^2 6 6^
(A.28)
(A.29)
or
VfV^ =
At
[a.+a^)
f
Sj Sq UqA/ = At'
,3 6,
(A.30)
(A.31)
and finally
121
2(OfOo)
At
6(sj So UqA/)
^ =(a/+«o)
■ = 2ar,+ar.
At'
Solving for a^ and a^
2(vjAt3Sf + 3^0 + lu^At)
Ae
2\^AtVf  35^ + 3^0 + v^At)
r' ;, * ^tjf.
(A.32)
^^2 »o+«/ (A.33)
(A.34)
«/ = X? • (^35)
APPENDIX B
SIMULATION SOURCE CODE
J!
123
'■ >■ ; " ; ■, . ,■ , 5.>
K
Source code for the simulations using MATLAB
124
autonomous. n
% Implementing path tracking on an Autonomous robot
% or Vehicle.
% Using the Position and Orientation information ONLY
% (No velocity information)
%
% Previous: particle. m
% Next: autonomousl .m
D2R = pi/180.; % Degrees to Radians conversion factor
r = 0.5;
t0 = 0.0; . '
Cx=1.0; .
Cy=0.0;
tf=6.20; % 6.5
delta = 0.05 ;
■ »■•■, '
% Initializing all the vector arrays
ta = [] ;
xOda= [] ; iv.,'
yOda= [] ; ■:
ro^a = []; • v^ ,.■,; i U> f,
rOya = [] ; ' ' '
r01c=[1.51; 0.05; 0] ;
thetac=1.7;
% The Loop starts over here for the duration tf
while tO<tf
rd = [Cx + r*cos (tO+delta) ; Cy+r*sin(tO+delta) ; 0];
they=atan2 (rd(2, 1) Cy, rd(l, 1) Cx) ;
if they<0
thetad=they + 2.0*pi + pi/2;
else
thetad = they+pi/2;
end; :
% Calculations for computing the required velocities
% Calculating the angular velocities and accelerations
V0c= [ 0; 0; (thetadthetac) /delta; (rd (1 , 1) rOlc (1 , 1) ) /delta; (rd (2 , 1)
r01c(2,l) )/delta;0] ;
w=[ V0c(3,l)] ;
V0=[V0c(4,l) V0c(5,l) 0];
r0=r01c+(Cross(w,V0)/V0c(3,l)*2) ' ;
%The location of the velocity pole is
rOx = rO(l,l) ;
rOy = r0(2,l);
ta = [ ta [ to + delta ] ] ;
rOxa = [ rOxa [ rOx ] ] ;
rOya = [ rOya [ rOy ] ] ;
xOda = [ xOda [ rd(l,l) ]];
125
yOda = [ yOda [ rd(2,l) ] ] ;
r01c=rd;
thetac=thetad;
to = to + delta;
end . '■
figure; ^ ' ■•
plot (rOxa,rOya, 'o' ) , title (' Path Tracking (pose) : Screw Locations
llQll 1 )
hold on
plot (xOda,yOda, 'r+' ) ,. /
%axis( [2 22 2])
legend ( 'pole locations' , 'path' ) ;
axis equal;
zoom on; 
grid on;
hold off
126
autonomous 1 .m
% MAIN PROGRAM
% Implementing path tracking on an Autonomous robot
% or Vehicle.
% Using the Position and Orientation information ONLY
% (No velocity information)
%
% Previous: autonomous. m
% (the only new thing is different starting point )
% Next: autonomous2 .m
D2R = pi/180.; % Degrees to Radians conversion factor
r = 0.5; • , .
to = 0.0; '■ ♦
Cx=1.0;
Cy=0.5;
tf=1.9*pi;
delta = 0.05; ,
% Initializing all the vector arrays
ta = []
xOda=[]
yOda=[]
rOxa = [] ;
rOya = [] ; '
thetada = [] ; ■ , .
r01c= [1.50; 0.5; 0] ;
thetac=pi/2;
% The Loop starts over here for the duration tf
while t0<tf
rd = [Cx+r*cos(tO+delta) ; Cy+r*sin(tO+delta) ; 0];
they=atan2 (rd(2,l) Cy, rd(l, 1) Cx) ;
if they<0
thetad=they + 2.0*pi + pi/2;
else '
thetad = they+pi/2;
end; . .,, „. ... I
% Calculations for computing the required velocities
% Calculating the angular velocities
V0c=[ 0; 0; (thetadthetac) /delta; (rd (1 , 1) rOlc (1 , 1) ) /delta; (rd (2 , 1)
r01c(2,l) )/delta;0] ; ...... ...
w=[ V0c(3,l) ] ;
V0=[V0c(4,l) V0c(5,l) 0];
rO= r01c+ (cross (w,V0)/V0c (3,1) *2) ' ;
%The location of the velocity pole is '. "
rOx = rO (1 , 1) ;
rOy = r0(2,l) ;
127
ta = [ ta [ to + delta ] ] ;
rOxa = [ rOxa [ rOx ] ] ;
rOya = [ rOya [ rOy ] ] ;
xOda = [ xOda [ rd(l,l) ]] ;
yOda = [ yOda [ rd(2,l) ] ] ;
thetada = [thetada [thetad] ] ;
r01c=rd;
thetac=thetad;
to = to + delta;
end
figure;
plot (rOxa,rOya, 'o' ) , title (' Path tracking (pose) : Screw Locations
"o" ' ) ...
hold on
plot (xOda , yOda , ' r+ ' )
legend ('pole location' , 'path' ) ;
axis equal; ..
%axis( [2 22 2]) ■
grid on;
zoom on • ,
hold off
•■3t . t
128
% autonomous 2 a. m
% Implementing Path Tracking on an Autonomous Robot/Vehicle
% specifying the initial position and the final
% position and velocity states
% Circular Path / Draws the orientation as a box
%
% Previous: autonomous. m
D2R = pi/180.; % Degrees to Radians conversion factor
r = 0.5; • •
to = 0.0;
Cx=1.0;
Cy=0.5;
tf=6.1 ; % original value 6.3 . ^. ,_^. .. ^
delta = 0.1; ,':'i:f*c '..•/■' '''I'
% Initializing all the vector arrays
ta = [] ; 
xOda= [] ;
yOda=[] ; : ■ ' ■" ■■'•■;./ '■ J ./
rOxa = [] ;
rOya = [] ;
thetada = [] ;
angaa= [] ;
axa= [] ;
aya= [] ;
omegaa= [] ;
vxa= [] ;
vya= [] ;
r01c=[1.54; .45; 0] ;
thetac=pi/2 + . 002 ;
% The Loop starts over here for the duration tf
while tO<tf
% path is specified as a circle at an offset (Cx, Cy) from the Origin
rd = [Cx+r*cos(tO+delta) ; Cy+r*sin(tO+delta) ; 0];
they=atan2(rd(2,l) Cy, rd(l, 1) Cx) ;
if they<0
thetad=they + 2.0*pi + pi/2;
else
thetad = they+pi/2;
end ;
vd = [r*sin(tO+delta) ;r*cos (tO+delta) ;0] ;
omegad= 1 ;
% Calculations for computing the required initial velocities
% Calculating the angular velocities and accelerations
V0c= [ 0; 0; 2* ( (thetadthetac) /delta) omegad; 2* ( (rd (1 , 1) 
r01c(l,l) ) /delta) vd(l,l) ;2* ( (rd(2, 1) rOlc (2, 1) ) /delta) vd(2,l) ;0] ;
129
As=[0; 0; (2/delta) * (omegad ( (1* (thetadthetac) ) /delta) ) ;
(2/delta) * (vd(l,l)  ( (1* (rd(l,l) r01c(l, 1) ) ) /delta) ) ;
(2/delta)*(vd(2,l)( (l*(rd(2,l)r01c(2,l)))/delta)) ; 0]
w= [ V0c(3,l) ] ;
V0=[V0c(4,l) V0c(5,l) 0] ;
rO= r01c+ (cross (w,VO)/VOc( 3, 1)^2) ' ;
%The location of the initial velocity pole is
rOx = rO(l,l) ;
rOy = r0(2,l) ;
ta = [ ta [ to + delta ] ] ; " ■
rOxa = [ rOxa [ rOx ] ] ;
rOya = [ rOya [ rOy ] ] ;
xOda = [ xOda [ rd(l,l) ]] ;
yOda = [ yOda [ rd(2,l) ]] ;
thetada = [thetada [thetad] ] ;
angaa = [angaa [As (3,1)]]; «•;, ■•
axa= [ axa [As (4,1)]]; " "^' ' <"'
aya= [ aya [As (5,1)]];
omegaa= [omegaa [V0c(3,l)]]; ] t
vxa= [vxa [V0c(4,l)]];
vya=[vya [V0c(5,l)]]; . , .^; , ^ , «, ■::
r01c=rd;
thetac=thetad;
to = to + delta;
end
figure;
plot (rOxa,rOya, 'o' ) , title ('Path tracking (Pose, Velocity) : Screw
Locations "o" ' )
hold on
for i = 1: length (xOda)
DrawBox( [xOda(i) ; yOda(i)], thetada (i) , 0.02, 0.01);
end
plot (xOda , yOda , ' g ' ) ;
legend ('pole locations');
%axis( [2 22 2])
axis equal;
zoom on;
hold off;
130
% autonomous 3 .n
% Autonomous Path Tracking
% specifying the initial position and the final
% position and velocity states: Initial velocity is NOT specified
% Implemented with a DrawBox function to show the change in
% orientation as the vehicle traverses the desired path (Hyperbolic
path)
% Previous: autonomous2 .m
D2R = pi/180.; % Degrees to Radians conversion factor
r = OS.
tO = 0.5; , . . ■ . .,■
Cx=1.0; , V
Cy=0.5; ■ 
tf = 0.5; , , .. ■■■■:^.
delta = 0.05; , ■•.,''.
% Initializing all the vector arrays
ta = []
xOda= []
yOda=[]
rOxa = [] ; "^
rOya = [] ;
thetada = [] ;
angaa= [] ;
axa= [] ;
aya= [] ;
omegaa= [] ;
vxa= [] ;
VYa= [] ;
r01c=[4; 8; 0] ; ,
thetac=0.002;
% The Loop starts over here for the duration tf
while tO<tf
% defining the trjectory of the desired motion
% rd = [Cx+r*cos(tO+delta) ; Cy+r*sin(tO+delta) ; 0]; % circle
% rd = [ 6 ; 5* (tO+delta) ; ]; % constant x, linear vel . y
rd = [ tO+delta ; 4* (tO+delta) *2 ; ]; % parabola
thetad = 5 . 0* (tO+delta) ;
vd = [1 ; 8*{t0+delta) ; 0] ; ' ' ..
omegad=5.0; •■;,... .. .
% Calculations for computing the required initial velocities
% Calculating the angular velocities and accelerations
VOc=[ 0; 0; 2* ( (thetadthetac) /delta) omegad; 2* ( (rd (1 , 1) 
r01c(l,l) ) /delta) vd(l,l) ;2*( (rd(2,l) r01c(2,l) ) /delta) vd(2,l) ;0] ;
As=[0; 0; 2* (thetadthetac) /delta*2 ; 2* (rd (1 , 1) 
r01c(l,l) )/delta*2;2*(rd(2,l) r01c(2,l) )/delta^2; 0] ;
131
w=[ V0c(3,l)] ;
V0=[V0c(4,l) V0c(5,l) 0] ;
rO= r01c+(cross(w,V0)/V0c(3,l)"2) ' ; ;\
%The location of the initial velocity pole is
rOx = rO{l,l) ;
rOy = r0(2,l) ;
ta = [ ta [ to + delta ] ] ;
rOxa = [ rOxa [ rOx ] ] ;
rOya = [ rOya [ rOy ] ] ;
xOda = [ xOda [ rd(l,l) ] ] ;
yOda = [ yOda [ rd(2,l) ]];
thetada = [thetada [thetad] ] ;
angaa = [angaa [As (3,1)]], •
axa= [ axa [As (4,1)]], •
aya= [ aya [As (5,1)]]; .
omegaa= [omegaa [VOc (3,1)]];
vxa= [vxa [VOc (4,1)] ] ;
vya=[vya [VOc (5,1)]] ;
r01c=rd;
thetac=thetad;
to = to + delta;
end
figure;
plot (rOxa,rOya, 'o' ) , title (' Path Tracking (Pose, Velocity) : Screw
locations "o" ' )
hold on
plot (xOda,yOda, 'c' )
legend ('pole locations ', 'path' ) ;
for i = 1 : length (xOda)
DrawBox( [xOda(i) ; yOda(i)], thetada (i) , .05, 0.025);
end
axis( [2 20.5 3.5])
zoom on ; ,
hold off
132
% inain4.in
% An example for a 3R planar serial chain to study
% the proposed path tracking methodology
% using initial position and final position data
% Program requires the following functions:
% / Screw / Draw3R
% Previous: main3.m
% Next : ? ?
% Last Update: June 10, 2001
D2R = pi/180.; % Degrees to Radians conversion factor
a_= [ 1 ; 1 ; 1] ; % Constant data, link lengths
% r is a constant required by the endeffector path,
r = 0.2;
% The initial and final time, and the time step are given below
to = 0.0;
tf=6.34;
delta =0.1;
%Current Position Data
re = [2; 1 ; 0] ;
thetac = 0;
[setlc, set2c, flagc] = reverse_position_3R{a_, re, thetac);
if flagc == 1
error ('The initial position is outside the manipulator workspace,
there is no point in completing the problem')
else
% Initializing all the vector arrays
ta = [] ;
rOxa = [] ;
rOya =[] ; . ,,^^, ,, ,, .,, ,. . ."; .
thetaa=[]; J I '^J "'"O > '.M i.r^ . .: «. ^
thetala=[]
theta2a= []
theta3a= []
w01a= []
wl2a= []
w23a=[]
ew01a= []
ewl2a= []
ew23a= []
%w01f=[]
% The Loop starts over here for the duration tf
while tO<tf
%The desired motion and velocity of the endeffector is given below
rd = [0.5+r*cos (tO+delta) ; r*sin(tO+delta) ; 0];
thetad = 0;
Vd= [0;0;0;r*sin(tO+delta) ;r*cos (tO+delta) ; 0] ;
% The following statement compute the reverse analysis of the
manipulator % for the required position at time tO+delta.
[setl, set2, flag] = reverse_position_3R (a_, rd, thetad);
if flag == 1
133
error ('The required position is outside the manipulator
workspace ' )
else
[Jd, waste] = Screw (a_, set 1) ;
% wd are the join angular velocities computed from the desired
% velocity state
wd=Jd\Vd;
% w are the join angular velocities computed from our tracking
% algorithm
w= (setlsetlc) /delta;
% Calculating Errors in Position, Orientation,
% Linear and Angular velocity
ta = [ ta [ to + delta ] ] ;
rOxa = [ rOxa [ rd(l,l) ]] ;
rOya = [ rOya [ rd(2,l) ] ] ;
thetaa = [thetaa [thetad] ] ; ■■ ■ :
thetala = [thetala [setl{l,l)]];
theta2a = [theta2a [setl(2,l)]];
thetaSa = [theta3a [setl(3,l)]];
w01a= [wOla [ w(l,l)] ] ; ^
wl2a=[wl2a [ w(2,l)]];
w23a= [w23a [ w(3,l) ] ] ;
ew01a=[ew01a [w(l, 1) wd(l, 1) ] ] ;
ewl2a= [ewl2a [w(2, 1) wd(2, 1) ] ] ;
ew23a=[ew23a [w(3, 1) wd(3, 1) ] ] ;
% Updating for the next iteration
to = to + delta; • '
setlc=setl;
end ■ . • ■■
end
% some plotting
figure;
plot (rOxa,rOya) , title ('Tracking of 3R manip. using inv. analysis
(Pose) ') ;
Draw3R(a_, thetala, theta2a, theta3a) ;
% plot(ta, wOla, ta, wl2a, ta, w23a) , title ( 'Joint Angular
Velocities ' ) ;
figure;
plot(ta, wOla, ta, wOla+ewOla, ta, wl2a, ta, wl2a+ewl2a, ta, w23a,
ta, w23a+ew23a) , title ( 'Actual and Desired Angular Velocities');
legend ( 'omegaOl ' , 'omega01_d', 'omegal2', 'omegal2_d', 'omega23', 'omega23_d
' ' '■
xlabel ( ' time ' ) ;
figure;
plot(ta, ewOla, '', ta, ewl2a, '', ta, ew23a, ':'), title ( 'Errors
in Joint Angular velocities');
legend ( 'error wOl', 'error wl2 ' , 'error w23 ' ) ;
xlabel ( ' time' ) ;
end
134
% inain54.ni
% An example for a 3R planar serial chain to study
% the proposed path tracking methodology
% using initial position and final position data
% Program requires the following functions:
% / Screw / Draw3R
% Previous: main4.ra
% Updated: June 15, 2001 ^ .
D2R = pi/180.; % Degrees to Radians conversion factor
a_= [ 1 ; 1 ; 1] ; % Constant data, link lengths
% r is a constant required by the endeffector path,
r = 0.2;
% The initial and final time, and the time step are given below
to = 0.0;
tf=6.34;
delta = 0.1 ;
%Current Position Data ' ' '
re = [0.72; 0.1 ; 0] ; ^. ,
thetac = 0;
[setlc, set2c, flagc] = reverse_position_3R(a_, re, thetac);
if flagc == 1
error ('The initial position is outside the manipulator workspace,
there is no point in completing the problem')
else
% Initializing all the vector arrays ,,
ta = [] ;
rOxa = [] ;
rOya = [] ; ' ■ _■
thetaa= [] ;
thetala=[]; theta2a=[]; theta3a= [] ;
w01a=[]; wl2a=[]; w23a=[];
alpha01a=[]; alphal2a= [] ; alpha23a= [] ;
ew01a=[]; ewl2a= [] ; ew23a= [] ; . v
eal01a=[]; eall2a=[]; eal23a= [] ; •'
% The Loop starts over here for the duration tf
while to<tf ... , ;,;.:"
%The desired motion and velocity of the endeffector is given below
rd = [0.5+r*cos( (tO+delta) ) ; r*sin( (tO+delta) ) ; 0];
thetad = 0.0;
Vd=[0;0;0;r*sin(tO+delta) ;r*cos(tO+delta) ; 0];
As= [0;0;0;r*cos(tO+delta) ;r*sin{tO+delta) ; 0];
wO=Vd (1:3,1) ;
vO=Vd(4:6,l) ; *"
CV=Cross (wO ' ,vO' ) ' ;
Ard=[As(l,l); As(2,l); As (3 , 1) ; As (4 , 1) CV (1 , 1) ; As (5 , 1)
CV(2,1) ;AS(6,1)CV(3,1)] ;
135
% The following statement compute the reverse analysis of the
manipulator % for the required position at time tO+delta.
[setl, set2, flag] = reverse_position_3R(a_, rd, thetad) ;
if flag == 1
error ('The required position is outside the manipulator
workspace ' )
else
[Jd, waste] = Screw {a_, setl) ;
% wd are the join angular velocities computed from the desired
% velocity state
wd=Jd\Vd;
Jwd=Jd*diag(wd) ;
alphad=Jd\ (ArdPaccst ( Jwd, 3,1));
% w are the join angular velocities and accelerations computed
from our
% tracking algorithm
w=2* (setlsetlc) /deltawd;
alpha=2* (setlsetlc)/delta*2;
% Calculating Errors in Position, Orientation, ''■, .
% Linear and Angular velocity
ta = [ ta [ to + delta ] ] ;
rOxa = [ rOxa [ rd(l,l) ] ] ;
rOya = [ rOya [ rd(2,l) ] ] ;
thetaa = [thetaa [thetad] ] ;
thetala = [thetala [setl(l,l)]];
theta2a = [theta2a [setl(2, 1) ] ] ;
thetaBa = [theta3a [setl(3 , 1) ] ] ;
w01a= [wOla [ w(l,l) ] ] ;
wl2a= [wl2a [ w(2,l)] ] ;
w23a=[w23a [ w(3,l)] ] ;
ew01a=[ew01a [w(l, 1) wd(l, 1) ] ] ;
ewl2a= [ewl2a [w(2, 1) wd(2, 1) ] ] ;
ew23a=[ew23a [w(3, 1) wd(3, 1) ] ] ;
alpha01a= [alphaOla [alpha(l,l)]]
alphal2a= [alphal2a [alpha(2,l)]]
alpha23a= [alpha23a [alpha (3, 1) ] ]
eal01a= [ealOla [alpha (1, 1) alphad(l,
eall2a= [eall2a [alpha(2, 1) alphad(2,
eal23a= [eal23a [alpha (3 , 1) alphad (3 ,
1)]] ;
1)]] ;
1)]];
% Updating for the next iteration
to = to + delta;
setlc=setl;
end
end
• 7^
% some plotting
figure;
plot (rOxa, rOya) , title ('Tracking of 3R manip. using inv. analysis
(pose, vel. ) ' ) ;
legend ( 'desired path');
136
Draw3R(a_,thetala, theta2a,theta3a) ;
figure;
plot(ta, wOla, ta, wOla+ewOla, ta, wl2a, ta, wl2a+ewl2a, . . .
ta, w23a, ta, w23a+ew23a) , ...
title ( 'Actual and Desired Angular Velocities');
legend ( 'omegaOl' , 'omega01_d' , 'omegal2' , 'omegal2_d' , . . .
'omega23 ' , 'omega23_d' ) ;
xlabel ( 'time' ) ;
figure;
plot(ta, ewOla, ta, ewl2a, ta, ew23a) , ...
title { 'Errors in Joint Angular Velocities');
legend (' error wOl', 'error wl2 ' , 'error w23 ' ) ;
xlabel ( ' time' ) ;
figure;
plot(ta, alphaOla, ta, alphaOla+ealOla, ta, alphal2a, ...
ta, alphal2a+eall2a, ta, alpha23a, ta, alpha23a+eal23a) , . . .
title ( 'Actual and Desired Angular Accelerations');
legend CalphaOl' , 'alpha01_d' , 'alphal2' , 'alphal2_d' , . . .
'alpha23' , 'alpha23_d') ;
xlabel ( 'time' ) ;
figure;
plot(ta, ealOla, ta, eall2a, ta, eal23a) , ...
title ( 'Errors in Joint Angular Accelerations');
legend ( 'error wOl' , ' error wl2 ' , 'error w23');
xlabel ( 'time' ) ;
% figure;
%plot{ta, wOla, ta, ta, wl2a, ta, w23a) , title ( 'Joint Angular
Velocities ' ) ;
%f igure;
%plot(ta, alphaOla, ta, alphal2a, ta, alpha23a) , titleCJoint
Angular Accelerations' ) ;
end
137
% Movplatlcn
% Program for path tracking of a parallel manipulator.
% Program uses the function reverse66.m to compute
% the Leg lengths, velocities, and accelerations
t=0;
% The initial pose of the platform off the path
cO_=[0 ; ; .5001] ;
cdO_=[0 ; ; ] ;
cdd0_=[0 ; ; ] ;
RMatO=[ cos(5*t^2+0.005) sin (5*t*2+0 . 005) 0; sin (5*t*2+0 . 005)
cos(5*t*2+0.005) 0; 1];
wm0=[0 10*t+0.05 0; 10*t + 0.05 ; 0] ;
amO=[0 10.0 0; 10 . 0; 0] ;
% The initial and final time for the tracking are
tO = 0.0; ..
tf=4.0;
delta=0.09; % TIME STEP
%Initialization of the arrays
ta=[];
Legsa= [] ;
VLegsa= []
ALegsa= []
Acoefa= []
Posea= [] ;
■ '\i
t'. • <<.
%Performing the Inverse Position and Velocity analysis for the initial
position and Velocity.
[LegsO, VLegsO, ALegsO] = reverse66 (cO_,RMatO, cd0_, wmO, cddO_, amO) ;
%The loop starts here
while t<tf
t=t+delta;
% The path to be followed by the moving platform
C_=[0 ; 0.1 ; 0.5 ] ;
Cd_= [ ; ; ] ;
cdd_=[0 ; ; ] ;
RMat= [cos(5*t*2) sin(5*t*2) 0; sin(5*t*2) cos(5*t*2) 0; 1] ;
wm=[0 10*t 0; 10*t 0; 0];
am=[0 10 0; 10 0; 0] ;
[Legs, VLegs, ALegs] = reverse66 (c_,RMat, cd_, wm, cdd_, am);
138
for i=l:6
•;
a3(i)=0.5*( 20*(Legs(i) LegsO(i) )  delta* (8*VLegs (
i)+12*VLegs0(i) )
+ delta*2* (ALegs(i)+7*ALegsO(i) )
)/delta"3;
a4(i)= ( 15* (Legs(i) LegsO (i) )  delta* (7*VLegs (i)
+8*VLegs0(i) ) ...
+ delta"2*(ALegs(i)+6*ALegsO(i) )
) /delta 4;
a5(i)=0.5*( 12* (Legs (i) LegsO (i) )  6*
delta* (VLegs (
i)+VLegs0(i) ) ...
+ delta*2*(ALegs(i)+5*ALegsO(i) )
)/delta^5;
end
%Updating the arrays
ta=[ta ; [t]] ;
Legsa= [Legsa ; [Legs] ] ;
VLegsa= [VLegsa ; [VLegs] ] ;
ALegsa= [ALegsa; [ALegs] ] ;
Acoef a= [Acoef a ;[a3(l) a4(l) a5(l) a3(2)
a4(2) a5(2) a3(3) a4(3) a5(3)
a3(4) a4(4) a5(4) a3(5) a4(5)
a5(5) a3(6)
a4(6) as (6)] ];
Posea=[Posea ;[RMat(l,:) RMat(2,:) RMatC
i, :) c_']] ;
%Update the information
cO_=c_; ^
cdO =cd ; ^.v .,i. fy . , , „
cddO_=cdd_; ■■ ^, V' ' ' ''^^ ^
\ntP^
■. . '■' If
RMatO=RMat;
wmO=wm;
amO=am;
end
figure;
axis equal;
plot(ta, Legsa (:,1), '', ta, Legsa ( :
,2), '', ta
Legsa(:,3), ':',
ta, Legsa ( : ,4) , '.',...
ta, Legsa(:,5), '+', ta, Legsa(:,6),
/ / ...
title ('Leg Displacements of the Platfc
3rm) ' ) ;
legendCLegKl) ' , 'Leg2(q) ' , 'Leg3(m) ' ,
'Leg4(r) ', ...
'Leg5(n) • , •Leg6(p) ' ) ;
%axis([0.0, 1.0, 10, 30]);
ylabel ( ' in. ' ) ;
xlabel ( 'time' ) ;
figure;
axis equal;
plot(ta, VLegsa (:,1), '', ta, VLegsa (:, 2), '', \
:a, VLegsa ( : ,3) ,
■ : ' , ta, VLegsa (:, 4) , •. ' , ...
ta, VLegsa (:, 5), '+', ta, VLegsa (:, 6)
, ■"'), ...
title ('Leg Velocities of the Platform
');
legendCLegKl) ' , 'Leg2(q) ' , 'Leg3(m) ' ,
'Leg4(r) ', ...
'Leg5(n) ' , 'Leg6(p) ') ;
%axis([0.0, 1.0, 15, 25]);
ylabel ( ' in. /sec' ) ;
xlabel ( 'time' ) ;
figure; '
139
axis equal;
plot(ta, ALegsa(:,l), '', ta, ALegsa(:,2), '', ta, ALegsa(:,3),
':', ta, ALegsa(:,4), '.',...
ta, ALegsa(:,5), '+', ta, ALegsa(:,6), '*'), ...
titleCLeg Accelerations of the Platform');
legend ('Legl(l) ' , 'Leg2(q) ' , 'LegSdn) ' , 'Leg4(r) ' , . . .
'Leg5(n) ' , 'Leg6(p) ' ) ;
%axis( [0.0, 1.0, 30, 50] ) ;
ylabel ( ' in. /sec*2 ' ) ;
xlabel ( ' time ' ) ;
*»»»*
140
% reverse66 .n
% previous: reverse_position66 .m (pos & vel)
%
% This function computes the leg (connector lengths) of
% the Special 66 parallel platform
% the platform is parametrized using
% SIDE: determines the side of the platform (equi. triangle)
% bscale: scales the distance of the pivot point as a fraction of
SIDE
% tsidescale: scales the side of the top platform
% tsacle: scales the distance of the pivot point on the top platform
% as a fraction of {tsidesacle*SIDE)
% hscale: determines the height of the top platform w.r.t base
% ( again, as a fraction of SIDE)
%
% The INPUTS are the center of the top platform (c_) and the rotation
% Matrix of the top platfom RMat . Also cd_ is the velocity of the
center of top plat
% and wm is the angular velocity of top plat (it is a skew symmetric
matrix)
% AND the size of the platform in terms of the params . shown above
% as a 5x1 vector.
% The OUTPUT vectors are the 6 leg lengths and the velocities of the 6
legs
function [Legs, VLegs, ALegs] = reverse_position_66 (c_, RMat, cd_, wm,
cdd_, am, size)
if nargin < 6
error ('66 inv. pos. analysis requires 6 or 7 inputs'), end
if nargin > 7
error ('66 inv. pos. analysis requires 6 or 7 inputs'), end
if nargin == 6
% Platform size
size (1
1)
=
3 0.0; %SIDE
size (2
1)
=
0.8
%bscale
size(3
1)
=
1.0
%tsidescale
size (4
1)
=
0.8
%tscale
size (5
1)
s
1.0
%hscale
end
,
SIDE=size(l,l) ;
bscale=size (2,1) ;
tsidescale=size (3, 1) ;
tscale=size (4,1) ;
hscale=size (5, 1) ;
% forming the transform matrix from c_ and RMat
% and from cd_ and wm
for i = l:3  ■, \. ■
for j=l:3
TMat ( i , j ) =RMat ( i , j ) ;  
TVMat ( i , j ) =wm ( i , j ) ;
TVMataux ( i , j ) =wm ( i , j ) ; ' „ , . ■
TAMat (i, j)=am(i, j) ; ,.,. ' ; ' ■
end
TMat(i,4)=c (i,l) ;
141
TVMat (i,4)=cd_(i,l) ; ■■■.;,
TVMataux(i,4) =0;
TAMat (i,4)=cdd_(i,l) ;
end;
TMat(4, :) = [0,0,0,1] ; '
TVMat (4, :) = [0,0,0,1] , •
TVMataux(4, :)=[0,0,0,0] ;
TAMat(4, :)=[0,0,0,1] ;
% Defining the joint locations on the base platform (fixed)
pl= [SIDE/2; SIDE/(2*sqrt(3) ) ; 0;1];
p2=[(lbscale)*SIDE/2; (bscale (1/3) ) *SIDE*sqrt (3) /2;0;1] ;
p3=[0;SIDE/sqrt(3) ;0;1] ;
p4=[bscale*SIDE/2;  (bscale (2/3) ) *SIDE*sqrt (3) /2 ; 0; 1];
p5= [SIDE/2; SIDE/ (2*sqrt(3) ) ;0;1] ;
p6=[SIDE*(bscale0.5) ; SIDE*sqrt (3) /6 ; ; 1];
pMat= [pi p2 p3 p4 p5 p6] ;
%pMat
% Defining the joint locations on the top platform w.r.t to the
% center of the top platform
tSIDE=tsidescale*SIDE;
ptl= [tscale*tSIDE/2 ; (tscale (2/3 ) ) *tSIDE*sqrt (3 ) /2 ; ; 1] ;
pt2=[tSIDE/2; tSIDE/ (2*sqrt (3 ) ) ;0;1] ;
pt3=[tSIDE*( tscale 0.5) ; tSIDE/ (2*sqrt (3) ) ;0;1] ;
pt4=[tSIDE/2; tSIDE*sqrt (3 ) /6 ; ; 1] ;
pt5=[ (ltscale)*tSIDE/2;  (tscale (1/3) ) *tSIDE*sqrt (3) /2;0;1] ;
pt6=[0; tSIDE/sqrt(3) ; 0; 1];
ptMat=[ptl pt2 pt3 pt4 pt5 pt6] ;
% Finding the joint locations on the top platform w.r.t the
% base platform (fixed)
ptMat f ix=TMat *ptMat ;
%ptMatfix
VMat=ptMatf ixpMat ;
VelMat=TVMat*ptMatfix; '
VelMataux=VelMat ( 1 : 3 , 1 : 6 ) ;
AccMat=TAMat*ptMatf ix+TVMataux* (TVMataux*ptMatf ix) ;
for i=l:6
Legs (i) =sqrt (VMat ( : , i) ' *VMat ( : , i) ) ;
end
Legmat= [Legs; Legs; Legs; Legs]; ^,
uVMat=VMat . /Legmat ;
for i=l:6
VLegs(i)=uVMat( : ,i) '*VelMat (: ,i) ; "■
ALegs ( i ) =uVMat ( : , i ) • *AccMat ( : , i) + (norm(VelMataux( : ,i) )*2/Legs(i) )
VLegs(i)*2/Legs(i) ;
end
142
% Movplat2b.iD
% Program for path tracking of a parallel manipulator.
% Program determines the forces in each leg also
% REQUIRES THE FUNCTION "reverse66f .m"
% Implementing a constant acceleration motion (translation) with
% the Weight of the top platform ignored W = 0.
t=o;  ■ .:
% The initial pose of the platform off the path
C0_=[0.2 ; 0.4 ; 0.62 + 0.06*t*2];
Cd0_=[0 ; ; 0.06*2*t+.01] ;
cdd0_=[0 ; ; 0.06*2+. 001 ] ;
RMatO=[ 1 0; 1 0; 1] ;
wm0= [0 0; 0; 0] ;
amO=[0 0; 0; 0] ;
% The initial and final time for the tracking are
tO=0.0;
tf=0.50;
delta=0.02; % TIME STEP 
%Initialization of the arrays
ta=[];
Legsa= [] ;
VLegsa= []
ALegsa= []
Acoefa= []
Posea=[]; '' ■ ,.^ ^ \ i ■■■' l'' ':*'V4
FLegsa= [] ; " * ' ■'' '■'' '' '
%Performing the Inverse Position and Velocity analysis for the initial
position and Velocity.
[LegsO, VLegsO, ALegsO, FLegsO] = reverse66f (cO_,RMatO, cdO_, wmO,
cddO_, amO) ;
%The loop starts here
while t<tf
t=t+delta;
% The path to be followed by the moving platform
C_=[0.2 ; 0.4 ; 0.6 + 0.06*t"2]; ',
Cd_=[ ; ; 0.06*2*t] ;
cdd_=[0 ; ; 0.06*2] ; • ,
RMat=[l 0; 1 0; 1] ;
wm= [0 0; 0; 0];
am= [0 0; 0; 0] ;
[Legs, VLegs, ALegs, FLegs] = reverse66f (c_,RMat, cd_, wm, cdd_, am) ;
for i=l:6
a3(i)=0.5*( 20* (Legs (i) LegsO (i) )  delta* (8*VLegs (i) +12*VLegsO (i) )
143
+ delta"2* (ALegs
(i)+7
*ALegsO(i) )
)/delta*3;
a4(i)= ( 15*(Legs{i)
LegsO(i))  delta* (7*VLegs (i)
+8*VLegsO(i) ) ...
+ delta*2* (ALegs
(i)+6
*ALegsO(i) )
) /delta 4;
a5(i)=0.5*( 12*(Legs(
L)LegsO(i))  6*
delta* (VLegs (
i)+VLegsO (i) ) ...
+ delta*2* (ALegs
(i)+5
* ALegs (i) )
)/delta*5;
end
%Updating the arrays
 " !.''* ■■
ta= [ta ; [t] ] ;
Legsa= [Legsa ; [Legs] ] ;
VLegsa= [VLegsa ; [VLegs] ]
/
■ '
ALegsa= [ ALegsa ; [ALegs ] ]
t
Acoef a= [Acoef a ;[a3(l) a4(l)
a5(l) a3(2)
a4(2) a5{2) a3(3) a4(3) a5(3)
a3(4) a4(4)
a5(4)
a3(5) a4(5)
a5(5) a3(6)
a4(6) a5(6)] ] ;
Posea= [Posea ; [RMat (1, : )
RMat (2, :) RMatC
, :) c_']] ;
FLegsa= [FLegsa; [FLegs ' ] ] ;
%Update the information
cO_=C_; .^ ,. .
' i vr
uv^v"f^'f^,^,rl
cdO_=cd_; ' * •' '
■''
cddO_=cdd_;
■at
t % ;■ "
. ^ : ;■*" '■ "
RMatO=RMat; . ^ J^^ [^
; , ' »i
wmO=wm;
amO=am; ^\ _ *. ; •'
' ^.•'
;V; 'r^H f
\ '■  % ,.
end
% some plotting
figure;
axis equal;
plot(ta, Legsa (:,1),
1 _ 1
1
ta, Legsa (:
,2), ■', ta
, Legsa ( : ,3) , ' : ' ,
ta, Legsa ( : ,4) , '  . ' , . .
.
ta, Legsa(:,5), 'V ,
ta.
Legsa ( : ,6) ,
1 •*■ 1 \
f 1 . . •
title ('Leg Displacements
of the Platform' ) ;
legend CLegKl) ' , 'Leg2(q)
' , 'Leg3(m) ' ,
'Leg4(r) • , . . .
'Leg5(n) ' , 'Leg6(p)
');
%axis([0.0, 1.0, 10,
30])
/
ylabel('m') ;
xlabel ( ' time ' ) ;
figure;
" .: :
axis equal;
plot(ta, VLegsa (:,1),
t _ '
, ta, VLegsa (: ,2) , '' ,
ta, VLegsa (: ,3) ,
' : • , ta, VLegsa (: ,4) , ' 
I
. .
ta, VLegsa (:, 5), ' V ,
ta.
VLegsa ( : ,6)
1 * 1 N
f It'''
title ('Leg Velocities
J of
the Platform
');
legend ( 'Legl(l) ' , 'Leg2 (q)
', 'Leg3(m) ',
•Leg4(r) ', ...
'Leg5(n) ' , 'Leg6(p)
•);
%axis([0.0, 1.0, 15,
25]);
xlabel ( 'time' ) ;
ylabel ( 'm/sec' ) ;
figure;
axis equal;
144
plot(ta, ALegsa(:,l), '', ta, ALegsa(:,2), '', ta, ALegsa(:,3),
:', ta, ALegsa(:,4), '.',...
ta, ALegsa(:,5), ' V , ta, ALegsa{:,6), '^'), ...
titleCLeg Accelerations of the Platform');
legendCLegKD • , 'Leg2(q) ' , 'Leg3(m) ' , 'Leg4(r) • , . . .
'Leg5(n) • , •Leg6(p) ') ;
%axis([0.0, 1.0, 30, 50]);
xlabel ( 'time' ) ;
ylabel('m/sec*2') ;
f igure ;
axis equal;
plot(ta, FLegsa(:,l), '', ta, FLegsa(:,2), '', ta, FLegsa(:,3),
' : ' , ta, FLegsa( :, 4), '.',...
ta, FLegsa(:,5), ' V , ta, FLegsa(:,6), '^'), ...
titleCLeg Forces of the Platform');
legendCLegKD ' , 'Leg2(q) ' , 'Leg3(m) ' , 'Leg4(r) ', .. .
'Leg5(n) ' , 'Leg6(p) ' ) ;
%axis([0.0, 1.0, 30, 50]);
xlabel ( • time ' ) ; . ; _
ylabel('N');
i r> :'\.ii ' *' ' ^ ^
145
% Movplat2d.in
% Program for path tracking of a parallel manipulator.
% Program determines the forces in each leg also
% REQUIRES THE FUNCTION "reverse66f .m"
% Implementing a rotation around c (about z) , no translation,
% and constant angular acceleration
% the Weight of the top platform ignored W = 0.
% IMPORTANT ***** change Weight in "reverse66f .m"
t = 0;
% The initial pose of the platform off the path
cO_=[0 ; ; .5001] ;
cdO_=[0 ; ; ] ;
cddO_= [0 ; ; ] ;
RMat0=[ cos(5*t"2+0.005) sin (5*t*2+0. 005) 0; Bin(5*t 2+0.005)
cos(5*t*2+0.005) 0; 1];
wmO=[0 10*t + 0.05 0; 10*t+0.05 ; 0] ;
amO=[0 10.0 0; 10.0 0; 0] ;
% The initial and final time for the tracking are
tO=0.0;
tf=0.50;
delta=0.02; % TIME STEP
%Initialization of the arrays
ta=[];
Legsa= [] ;
VLegsa= []
ALegsa= []
Acoefa= []
Posea= [] ; „■
FLegsa= [] ;
%Performing the Inverse Position and Velocity analysis for the initial
position and Velocity.
[LegsO, VLegsO, ALegsO, FLegs0]= reverse66f (c0_,RMat0, cdO_, wmO,
cddO_, amO) ;
%The loop starts here
while t<tf
t=t+delta;
% The path to be followed by the moving platform
c_=[0 ; 0.1 ; 0.5 ] ;
cd_= [ ; ; ] ;
cdd_=[0 ; ; ] ;
RMat=[cos(5*t"2) sin(5*t*2) 0; sin(5*t*2) cos(5*t"2) 0; 1] ;
wm=[0 10*t 0; 10*t 0; 0];
am=[0 10 0; 10 0; 0] ;
[Legs, VLegs, ALegs, FLegs] = reverse66f (c_,RMat, cd_, wm, cdd_, am);
146
for i=l:6
a3{i)=0.5*( 20* {Legs(i) LegsO (i) )  delta* (8
* VLegs (
i)+12
*VLegsO(i) )
+ delta*2* (ALegs(i)+7*ALegsO(i) )
) /delt
a*3;
a4(i)= ( 15* (Legs (i) LegsO (i) )  delta* (7*VLegs (i)
+8*VLegs0(i) ) ...
+ delta*2* (ALegs(i)+6*ALegsO (i) )
)/delt
a"4;
a5(i)=0.5*( 12* (Legs (i) LegsO (i) )  6"
delta*
(VLegs (
i)+VLegs0(i) ) ...
+ delta*2* (ALegs(i)+5*ALegsO(i) )
)/delt
a*5;
end
%Updating the arrays
ta=[ta ; [t] ] ;
Legsa= [Legsa ; [Legs] ] ;
VLegsa= [VLegsa ; [VLegs] ] ;
ALegsa= [ALegsa; [ALegs] ] ;
Acoef a= [Acoef a ; [a3 (1) a4(l) a5(l) a3(2)
a4(2)
a5(2) a
i3(3)
a4(3) a5(3)
a3(4) a4(4) a5(4) a3 (5) a4(5
a5(5)
a3(6)
a4(6)
a5(6)] ];
Posea=[Posea ;[RMat(l,:) RMat(2,:) RMat(3,:) c_
•]];
FLegsa= [FLegsa; [FLegs']];
%Update the information
cO_=c_;
^
cdO_=cd_;
cddO_=cdd_;
RMatO=RMat;
wmO=wm;
amO=am;
end
% some plotting
figure;
axis equal;
plot(ta, Legsa (:,1), '', ta, Legsa ( :
,2), ■
■', ta
, Legsa ( : ,3) , ' : ' ,
ta, Legsa(:,4), '.', ...
ta, Legsa{:,5), 'V , ta, Legsa(:,6),
'"'),
title('Leg Displacements of the Platform');
legendCLegld) ', 'Leg2(q) ', ■Leg3(m) ',
•Leg4(r) ■ , . . .
'Leg5(n) ' , 'Leg6(p) ') ;
%axis([0.0, 1.0, 10, 30]);
ylabel ( 'm' ) ;
xlabel('time') ;
figure; '" •  ^
axis equal;
plot(ta, VLegsa (:,1), '', ta, VLegsa (:, 2), '', ta, VLegsa (:, 3),
' : ' , ta, VLegsa ( : ,4) , '.',...
ta, VLegsa (:, 5), 'v', ta, VLegsa (:, 6), '^'), ...
title ('Leg Velocities of the Platform');
legendCLegld) ' , •Leg2(q) ' , 'Leg3 (m) ' , •Leg4(r) ' , . . .
'Leg5(n) ' , 'Leg6(p) ') ;
%axis([0.0, 1.0, 15, 25]);
xlabel ( 'time' ) ;
ylabel ( 'm/seC ) ;
147
figure;
axis equal ;
plot{ta, ALegsa(:,l), '', ta, ALegsa(:,2), '
' : ' , ta, ALegsa(: ,4) , '.',...
ta, ALegsa(:,5), ' V , ta, ALegsa(:,6), '*'), ••
titleCLeg Accelerations of the Platform');
legendCLegKD ' , 'Leg2(q) ' , 'Leg3 (m) ' , 'Leg4(r) ' ,
'Leg5(n) • , 'LegeCp) ') ;
%axis( [0.0, 1.0, 30, 50] ) ;
xlabel ( 'time' ) ;
ylabel('m/sec*2') ;
ta, ALegsa( : ,3) ,
figure; '
axis equal;
plot(ta, FLegsa(:,l), '', ta, FLegsa(:,2), '
' : ' , ta, FLegsa(: ,4),'.',...
ta, FLegsa(:,5), ' V , ta, FLegsa(:,6), '*'),
title ('Leg Forces of the Platform');
legendCLegKD ' , 'Leg2(q) ' , 'Leg3(m) ' , 'Leg4(r)
'Leg5(n) ' , 'Leg6(p) ') ;
%axis([0.0, 1.0, 30, 50]);
xlabel ( 'time' ) ;
ylabel('N');
ta, FLegsa( : , 3) ,
r*^ V
148
reverse66f .n
% previous: reverse66.m (pos, vel and accel)
%
% This function computes the leg (connector lengths) of
% the Special 66 parallel platform ALSO computes the
% leg velocities, leg accelerations, and LEG FORCES
% the platform is parametrized using
% SIDE: determines the side of the platform (equi. triangle)
% bscale: scales the distance of the pivot point as a fraction of
SIDE
% tsidescale: scales the side of the top platform
% tsacle: scales the distance of the pivot point on the top platform
% as a fraction of (tsidesacle*SIDE)
% hscale: determines the height of the top platform w.r.t base
% ( again, as a fraction of SIDE)
%
% The INPUTS are the center of the top platform (c_) and the rotation
% Matrix of the top platfom RMat. Also cd_ is the velocity of the
center of top plat
% and wm is the angular velocity of top plat (it is a skew symmetric
matrix)
% AND the size of the platform in terms of the params. shovm above
% as a 5x1 vector.
% The OUTPUT vectors are the 6 leg lengths and the velocities of the 6
legs •
% The acceleration and forces in each of the legs
function [Legs, VLegs, ALegs, FLegs] = reverse_position_66 (c_, RMat,
cd_, wm, cdd_, am, size)
if nargin < 6
error ('66 inv. pos. analysis requires 6 or 7 inputs'), end
if nargin > 7
error ('66 inv. pos. analysis requires 6 or 7 inputs'), end
I2M=0.0254; % in to meter
if nargin ==6 . '
% Platform size
size (1,1) = 30.0*I2M; %SIDE
size(2,l) = 0.8
size(3,l) = 1.0
size(4,l) = 0.8
size(5,l) = 1.0
end
%bscale
%tsidescale
%tscale
%hscale
% Defining the Inertia Matrix (Mass in kgm, and Inertia in Kgmm*2)
% For explanation see Rico notes
I=[0 79.5928 0; 79.5928 ; 79.5928; 3.39146
0;...
3.39167 0; 00 6.590532 000];
% Computing the Velocity and Reduced Acceleration States
V=[wm(3,2) ; wm(l,3); wm(2,l); cd_(l,l); cd_(2,l); cd_(3,l)];
% Dual part of the Reduced accel vector aOw X v
DAR=cdd_wm*cd_;
AR=[am(3,2) ; am(l,3); am(2,l); DAR(1,1); DAR(2,1); DAR(3,1)];
149
■ ■"' ■
% Computing the Inertia Forces (weight
for scr's platform)
Weight=[0; 0; 0; ; 0; 0]; % W(k) = •
795.928
% Wrench on the top platform w.r.t center of moving platform
(moving
ccs)
Wc=I*AR+Lie(V,I*V) Weight ;
%This is an skew matrix for computing 1
the cross product
of rc/C.
Mc=[0 c_(3,l) c_(2,l); c_(3,l) c_(l,l); c_(2,l) c_
(1,1)
0];
% Calculating MC = Mc + r X F
Temp=Wc(4:6,l)+Mc*Wc(l:3,l) ;
% Wrench on the top platform w.r.t the
fixed coordinate
syst
5m
% by augmenting the F from Wc and MC f
rom Temp (moment component)
W= [Wc (1 : 3 , 1 ) ; Temp (1:3,1)];
SIDE=size(l,l) ;
bscale=size (2, 1) ;
tsidescale=size(3,l) ;
tscale=size(4, 1) ;
hscale=size(5, 1) ;
% forming the transform matrix from c_
and RMat
% and from cd_ and wm
for i=l:3
for j =1:3 r ; ^ ■^;'■J; p
ht ■
TMat (i, j)=RMat (i, j) ;
TVMat ( i , j ) =wm ( i , j ) ;
TVMataux ( i , j ) =wm ( i , j ) ;
TAMat(i, j)=am(i, j) ;
end
TMat(i,4)=c_(i,l) ;
TVMat (i,4)=cd_(i,l) ;
TVMataux (i, 4 )=0; 
TAMat (i,4)=cdd_(i,l) ;
end;
TMat(4, :)=[0, 0,0,1] ;
TVMat (4, :) = [0,0, 0,1] ;
TVMataux (4, :)= [0,0,0,0] ;
TAMat (4, : )=[0, 0,0,1] ;
% Defining the joint locations on the
base platform (fixed)
pl= [SIDE/2; SIDE/(2*sqrt(3) ) ; 0;1];
p2=[(lbscale)*SIDE/2; (bscale (1/3) )*
SIDE*sqrt (3) /2;0
1];
p3=[0;SIDE/sqrt(3) ;0;1] ;
p4=[bscale*SIDE/2;  (bscale (2/3) ) *SIDE*sqrt (3) /2; 0;
1];
p5=[ SIDE/2; SIDE/ (2*sqrt (3 ) ) ;0;1] ;
p6=[SIDE*(bscale0.5) ; SIDE*sqrt (3) /6
; ; 1] ;
pMat= [pi p2 p3 p4 p5 p6] ;
%pMat
% Defining the joint locations on the
top platform w.r
.t to
the
% center of the top platform
tSIDE=tsidescale*SIDE;
ptl=[tscale*tSIDE/2; (tscale (2/3) ) *tSIDE*sqrt (3) /2; 0;!
L];
150
pt2=[tSIDE/2; tSIDE/ (2*sqrt (3 ) ) ; ; 1] ; '
pt3=[tSIDE*(tscale0.5) ; tSIDE/ (2*sqrt (3) ) ; ; 1] ;
pt4=[tSIDE/2; tSIDE*sqrt (3) /6 ; ; 1] ;
pt5=[(ltscale)*tSIDE/2;  (tscale (1/3) ) *tSIDE*sqrt (3) /2;0;1] ;
pt6=[0; tSIDE/sqrt(3) ; 0; 1];
ptMat= [ptl pt2 pt3 pt4 pt5 pt6] ;
% Finding the joint locations on the top platform w.r.t to the center
of
% the top (moving) platform, expressed in the fixed coordinate system.
ptMatf ix=TMat*ptMat ;
%ptMatfix
VMat=ptMatfixpMat;
VelMat=TVMat*ptMatfix;
VelMataux=VelMat (1:3,1:6) ;
AccMat=TAMat*ptMatf ix+TVMataux* (TVMataux*ptMatf ix) ;
for i=l:6
Legs(i)=sqrt (VMat(: ,i) '*VMat ( : ,i) ) ; ■ >
end
Legmat= [Legs; Legs; Legs; Legs];
%VMat J.:
%Legmat
%VelMat
uVMat=VMat . /Legmat ;
%uVMat
for i=l:6
JL(:,i)=Cross((c_+ ptMatf ix(l : 3, i) )' ,uVMat (1 :3, i) ')' ; % Dual part of
the screw
VLegs ( i ) =uVMat ( : , i) ' *VelMat ( : , i) ;
ALegs ( i ) =uVMat ( : ,i) ' *AccMat ( : , i) + (norm(VelMataux( : ,i) ) ^2/Legs(i) ) . .
VLegs (i)*2/Legs(i) ;
end
J= [uVMat (1:3, :) ; JL] ; % The Jacobian Matrix
%Solve the force analysis
FLegs=J\W; % The force is in Newtons
151
% inaln54cPIDl.ir
July 2000
An example for a 3R planar serial chain to study
the proposed path tracking methodology
using initial position and final position and velocity data
Program requires the following functions:
/ Screw / Draw3R
Indues a PID Controller for a 3R Manipulator
Previous: main54c.m
Last Update: June 15, 2001
D2R = pi/180.; % Degrees to Radians conversion factor
%a = [ 1 ; 1 ; 1] ; % Constant data, link lengths
a_= [ .3 ; .3 ; .15] ;
r = 0.1; % r is a constant required by the endeffector path.
% The initial and final time, and the time step are given below
to = 0.0;
tf=2.0;
delta = 0.01 ; .
% MOTOR Data
K = 1; % Gain for the system (motor)
tau = [ 0.005753 ; 0.00063 ; 0.0017 ]; % Motor time constants
% Kp = [ .75 ; 0.5 ; .426 ]; % Proportional gains (from sysmodelx.m)
% Kd = [ .0002 ; 0.000166 ; .0001076 ] ; % Derivative gains
% Ki = [ 285.8 ; 199.84 ; 200.33 ] ; % Integral gains
Kp = [ 1.077 ; 0.6156 ; 1.0626 ]; % Proportional gains (from
sysmodelx.m)
Kd = [ .000404 ; 0.000166 ; .0001076 ] ; % Derivative gains
Ki =[ 71.8 ; 56.84 ; 262.33 ] ; % Integral gains
D = 0.03; % Delay in seconds
% variables
deltac = delta/10; % Determines the speed at which the PID control
loop will run
% Constant for the DISCRETE CONTROLLER
kO = Kp + Ki*deltac + Kd/deltac;
kl = Kp  (2*Kd/deltac) ;
k2 = Kd/deltac;
for i=l:l:3
el(i)=Oi
e2(i)^
e3(i)^
end
for
i=l:l: (2+round(D/delta) ) %
xl(i)=0;
x2(i)^
x3(i)^
end
for
end
i=l:l:2
wal(i)=0
wa2 (i) =0
wa3 (i) =0
152
%Current Position Data: initial position before starting
re = [0.5995 ; 0.01 ; 0] ;
thetac = ;
[setlc, set2c, flagc] = reverse_position_3R(a_, re, thetac);
if flagc == 1
error ('The initial position is outside the manipulator workspace,
there is no point in completing the problem')
else
% Initializing all the vector arrays
ta = [] ;
rOxa = [] ;
rOya = [] ;
thetaa= [] ;
thetala=[]; theta2a= [] ; theta3a= [] ;
w01a=[]; wl2a=[]; w23a= [] ,■
alpha01a= [] ; alphal2a= [] ; alpha23a= [] ;
ew01a=[]; ewl2a= [] ; ew23a=[];
eal01a=[]; eall2a= [] ; eal23a=[];
w01f=[]; wl2f=[]; w23f=[];
Wal=[]; Wa2=[]; Wa3= [] ;
errorwl=[]; errorw2= [] ; errorw3= [] ;
% The Loop starts over here for the duration tf
while tO<tf
%The desired motion and velocity of the endeffector is given below
rd = [0.5+r*cos( (tO+delta) ) ; r*sin( (tO+delta) ) ; 0] ;
thetad =0.0;
Vd= [0;0;0;r*sin(tO+delta) ;r*cos (tO+delta) ; 0] ;
As=[0;0;0;r*cos(tO+delta) ;r*sin(tO+delta) ; 0];
wO=Vd(l:3,l) ; . ;
vO=Vd(4:6,l) ; ' " •"" '
CV=Cross(wO' ,vO' ) ' ;
Ard=[As(l,l); As(2,l); As (3 , 1) ; As (4 , 1) CV(1 , 1) ;As (5 , 1) 
CV(2,1) ;AS(6,1)CV{3,1)] ;
% The following statement compute the reverse analysis of the
manipulator
% for the required position at time tO+delta.
[setl, set2, flag] = reverse_position_3R(a_, rd, thetad);
if flag == 1
error ( ' The required position is outside the manipulator
workspace' ) . .
else
[Jd, waste] = Screw (a_, setl) ;
% wd are the join angular velocities computed from the desired
% velocity state
wd=Jd\Vd; ' , .
Jwd=Jd*diag(wd) ;
alphad=Jd\ (ArdPaccst ( Jwd, 3 , 1) ) ;
153
% w are the
join angular velocities
and acce
ilerations computed
from
our
% tracking a
w=2* (setlse
%alpha=2* (s€
ilgorithm
itlc) /deltawd;
>tlsetlc)/delta"2;
alpha= (2/delta) * (wd ( (setlsetlc) /d(
2lta)) ;
% PID CONTROLLER WITH THE PLANT
tcl=0;
while ( tcl
< delta )
el(l) =
w(l,l)  wal(l) ;
e2{l) =
w(2,l)  wa2(l) ;
e3(l) =
w(3,l)  wa3(l) ;
xl(l) =
xl(2) + kO(l,l)*el(l) +
kl(l,l)
*el(2) +
k2(l
l)*el(3) ;
x2(l) =
x2(2) + k0(2,l)*e2(l) +
kl(2,l)
*e2(2) +
k2(2
l)*e2(3) ;
x3(l) =
x3 (2) + k0(3,l)*e3 (1) +
kl(3,l)
*e3(2) +
k2(3
l)*e3(3) ;
wal(l)
= exp(deltac/tau(l,l) )*
wal (2) +
K*(l  exp(
cielt<
ac/tau(l,l) ) )
*xl(l+round(D/delta) ) ;
wa2 (1)
= exp(deltac/tau(2,l) )*
wa2(2) +
K* (1  exp ( 
delt
ac/tau(2,l) ) )
*x2 {l+round(D/delta) ) ;
wa3(l)
= exp(deltac/tau(3,l) )*
wa3(2) +
K* (1  exp(
delt
ac/tau{3,l)))
wal(2)
wa2(2)
wa3(2)
*x3 (l+round(D/delta) ) ;
= wal(l) ;
= wa2(l) ;
= wa3 (1) ;
el(3) =
el(2); el(2) =
el(l);
62(3) =
e2(2); e2(2) =
e2(l) ;
e3{3) =
e3(2); e3(2) =
e3(l);
for j =
length (xl) :l:2
xl(
j) = xl(jl);
x2{
j) = x2(jl);
x3(
j) = x3(jl);
end
tcl = tcl + deltac;
end
% Array of
actual velocities (from
the control loop)
Wal = [ Wal
[ wal(l) ]] ;
Wa2 = [ Wa2
[ wa2(l) ]] ;
Wa3 = [ Wa3
[ wa3(l) ]] ;
% Array of
velocity errors (between actual
ang vel . ' wa ' and
command velocity
'w')
errorwl = 
errorwl [ wal(l)  w(l,
1) ]];
errorw2 = 
errorw2 [ wa2(l)  w(2,
1) ]];
errorw3 = 1
errorw3 [ wa3(l)  w(3,
1) ]] ;
wf=w+alpha*
delta;
154
% Calculating Errors in Position, Orientation,
% Linear and Angular velocity
ta = [ ta [ to + delta ] ] ;
rOxa = [ rOxa [ rd(l,l) ] ] ; '
rOya = [ rOya [ rd(2,l) ] ] ;
thetaa = [thetaa [thetad] ] ;
thetala = [thetala [setl(l,l)]]
theta2a = [theta2a [setl(2,l)]]
theta3a = [theta3a [setl(3,l)]]
w01a=[w01a [ w(l,l)] ] ;
wl2a= [wl2a [ w(2,l) ] ] ;
w23a=[w23a [ w(3,l)] ] ;
w01f=[w01f [wf(l,l)]];
Wl2f=[wl2f [wf(2,l)]];
w23f=[w23f [wf(3,l)]];
ew01a=[ew01a [wf (1, 1) wd(l, 1) ] ]
ewl2a=[ewl2a [wf (2, 1) wd(2 , 1) ] ]
ew23a=[ew23a [wf (3, 1) wd(3 , 1) ] ]
% Updating for the next iteration ;.
to = to + delta; tn ' ^i
setlc=setl;
end
end
figure;
axis normal;
plot(ta, wOla, '', ta, Wal, '', ta, wl2a, '.', ta, Wa2,
w23a, ' : ' , ta, Wa3, ' ' ) , ...
title ( 'Actual and Command Angular Velocities');
legend ( ' omegaOl ' , ' omega01_c ' , ' omegal2 ' , ' omegal2_c ' ,
' omega23 ' , ' omega23_c ' ) ;
xlabel ( 'time' ) ;
figure;
plotita, errorwl, '', ta, errorw2, '', ta, errorw3, ':')
title ( 'Errors in Joint Angular Velocities');
legend ( ' error wl ' , ' error w2 ' , ' error w3 ' ) ;
xlabel { 'time' ) ; .
ta.
%f igure;
%plot(ta, wOla, ta, ta, wl2a, ta, w23a) , title ( 'Joint Angular
Velocities ' ) ;
% figure;
%plot(ta, alphaOla, ta, alphal2a, ta, alpha23a) , titleCJoint
Angular Accelerations' ) ;
 LIST OF REFERENCES
AbbOO
Abbasi, W. A., Ridgeway, S. C, Adsit, P. D., Crane, C. D., and Duffy, J.,
"Investigation of a Special 66 Parallel Platform for Contour Milling." Trans,
of the ASME, Journal of Manufacturing Science and Engineering, Vol. 122,
No. 1, February 2000, pp: 132139.
Abe92
Abe, S. and Tsuchiya, T., "Robot Manipulator Path Control Based on Variable
Speed Trajectory Planning." Advanced Robotics, Vol. 6, No. 1, 1992, pp: 1
13.
Ami96
Amin, S. and Ahtiwash, 0. M., "Performance of Two Neuro Controllers for
Robot Path Planning Control." Proc. 1996 IEEE 22"'' Intl. Conf on Industrial
Electronics, Control, and Instrumentation, August 1996, Taipei, Taiwan, Vol.
3,pp: 18561861.
BalOO
Ball, R. S., .4 Treatise on the Theory of Screws, Cambridge University Press,
1900. Paperback edition, 1999.
B0I88
Bollinger, John G. and Duffie, Neil A., Computer Control of Machines and
Processes, Addison Wesley Publishing Company, 1988.
Cah96
Cahill, A. J., Kieffer, J. C, and James, M. R., "Robust TimeOptimal
Trajectory Planning for Robot Manipulators." IEEE Robotics and Automation
Conference, 1996.
Cha30
Chasles, M., "Note sur les proprietes generales du systeme de deux corps
semblables entr'eux et places d'une maniere quelconque dans I'espace; et sur
le deplacement fmi ou infmiment petit d'un corps solide libre." Ferussac,
Bulletin des Sciences Mathematiques, Vol. xiv., pp: 321326, 1830.
Cra99
Crane, C. and Duffy, J., "Geometry of Mechanisms and Robots II," Course
notes for EML 6282, Graduate Course, Dept. of Mechanical Engineering,
University of Florida, 1999.
Du95
Du, S., Tang, J., and Yang, G., "A New Method of Discrete Trajectory
Plarming for Robot Manipulators. " International Journal of Robotics and
^Mtowaf/on, Vol. 10, No. 4, 1995, pp: 123129.
Fla88
Flash, T. and Potts, R. B., "Discrete Trajectory Planning." International
Journal of Robotic Research, Vol. 6, No. 1, 1988, pp: 4857.
155
156
Gra93 Grasa, P., Volberg, 0., and Polyakhov, N., "Direct Adaptive Trajectory
Control of Robot Manipulators via Fuzzy Logic Algorithm." Proc. 8"' Intl.
Conf. on Applications of AI in Engineering, Toulouse, France, Vol. 2, 1993,
pp: 561569.
Gri93 Griffis, M. and Duffy, J., "Method and Apparatus for Controlling
Geometrically Simple Parallel Mechanisms with Distinctive Connections." US
Patent No. 5,179,525, January 12, 1993.
Hun78 Hunt, K. H., Kinematic Geometry of Mechanisms, Oxford University Press,
1978.
Kie96 Kieffer, J. C, Cahill, A. J., and James, M. R., "ClosedLoop Trajectory
Generation for Robust TimeOptimal Path Tracking." Proc. of the 1996 IEEE
International Conf On Robotics & Automation." Minneapolis, MN, April
1996, pp: 15591565.
Kie97 Kieffer, J., Cahill, A. J., and James, M. R., "Robust and Accurate Time
Optimal PathTracking Control for Robot Manipulators." IEEE Trans.
Robotics and Automation, Vol. 13, No. 6, Dec. 1997, 880890.
Li98 Li, Q., Tso, S. K., and Zhang, W. J., "Control of Robot Manipulators Using a
DualModelBased Structure." Proc. of 1 998 ASME DETC, September 1316,
Atlanta, GA.
Lin83 Lin, C. S., Chang, P. R., and Luh, J. Y. S., "Formulation and Optimization of
Cubic Polynomial Joint Trajectories for Industrial Robots," IEEE Trans, on
Automatic Control, Vol. AC28, No. 12, 1983, pp:10061017.
Luh77 Luh, J. Y. S. and Walker, M. W., "Minimumtime Along the Path for a
Mechanical Arm." Proc. 16"' Conf Decision Contr., Dec. 1977, pp:755759.
LuhSl Luh, J. Y. S. and Lin, C. S., "Optimum Path Planning for Mechanical
Manipulators." ASME Journal of Dynamical Systems, Measurement, and
Control., Vol. 102, June 1981, pp: 142151.
Mar96 Martinez, J. M. R. and Duffy, J., "An Application of Screw Algebra to the
Acceleration Analysis of Serial Chains," Mechanism and Machine Theory,
Vol. 31, No. 4, pp: 445457, 1996.
Mas99 Mason, P. A. C, Walchko, K., and Crane, C. D., "A MIMO Fuzzy Controller,
for Tracking: Robot Control." 1999 Florida Conf. on Recent Advances in
Robotics, April 1999, Gainesville, FL.
http://cimar.me.ufl.edu/FLA99/papers.html .
157
Moo79 Moore, R. E., Methods and Applications of Interval Analysis. Philadelphia,
PA. SI AM Press, 1979.
Naj96 Najson, F. and Kreindler, E., "Robot Robust Path Tracking." Dynamics and
Control, Vol. 6, 1996, pp: 333359.
Oga97 Ogata, K., Modern Control Engineering, 3"* Edition, PrenticeHall, 1997.
Par97 Park, S. and Park, C. H., "Modelbased Path Planning and Tracking Control
Using Neural Networks for a Robot Manipulator." Conf. Proc. of the 1997
IEEE Intl. Conf on Neural Networks, Houston, TX, Vol. 3, June 1997, pp:
17611765.
Pau81 Paul, R. P. C, Robot Manipulators: Mathematics, Programming, and Control.
Cambridge, MA. M.I.T Press, 1981.
Phi84 Phillips, J., Freedom in Machinery. Vol. I, Introducing Screw Theory,
Cambridge University Press, 1984.
Pia97 Piazzi, A., and Visioli, A., "An Interval Algorithm for MinimumJerk
Trajectory Planning of Robot Manipulators. " Proc. Of the 36^'' Conf On
Decision & Control, San Diego, CA, USA, December 1997.
Poi06 Poinsot, L., "Sur la composition des moments et la composition des aires"
(1804). Journal de I'Ecole Poly technique. Vol. vi. (13 ch.), pp: 182205,
1806.
Rag93 Raghavan, M., "The Stewart Platform of General Geometry has 40
Configurations." Trans, of the ASME, Journal of Mechanical Design, Vol.
115,June 1993, pp: 272282.
Rel Reliance Motor Control, Inc., DC Motors, Speed Controls, Servo Systems: The
ElectroCraft Engineering Handbook, Reliance Motor Control Inc., Eden
Prairie, Mirmesota.
Ric99 Rico, J. M., Gallardo, J., and Duffy, J., "Screw Theory and Higher Order
Kinematic Analysis of Open Serial and Closed Chains." Mechanism and
Machine Theory, Vol. 34, pp: 559586, 1999.
Shi85 Shin, Kang G. and McKay, Neil D., "MinimumTime Control of Robotic
Manipulators with Geometric Path Constraints. " IEEE Transactions on
Automatic Control, Vol. AC30, No. 6, 1985, pp: 531541.
158
Shi86a
Shin, Kang G. and McKay, Neil D., "A Dynamic Programming Approach to
Trajectory Planning of Robotic Manipulators." IEEE Transactions on
Automatic Control, Vol. AC31, No. 6, 1986, pp:49 1500.
Shi86b
Shin, Kang G. and McKay, Neil D., "Selection of NearMinimum Time
Geometric Paths for Robot Manipulators. " IEEE Transactions on Automatic
Conrro/, Vol. AC31, No. 6, 1986, pp: 501511.
Shi87
Shin, K. J. and McKay, N. D., "Robust Trajectory Planning for Robotic
Manipulators Under Payload Uncertainties." IEEE Trans. Automatic Control,
Vol. AC32, No. 12, 1987, pp: 10441054.
Slo87
Slotine, J. J. E. and Li, W., "On the Adaptive Control of Robot Manipulators."
InternationalJournal of Robotics Research, Vol. 6, 1987, pp: 4959.
Ste65
Stewart, D., "A Platform with Six Degrees of Freedom," Proc. Institution of
Mechanical Engineers, Vol. 180, Pt. 1, No. 15, 1965, pp: 371386.
Tan88
Tan, H. H. and Potts, R. B., "Minimum Time Trajectory Planner for the
Discrete Dynamic robot Model with Dynamic Constraints." IEEE Journal of
Robotics and Automation, Vol. 4, No. 2, 1988, pp: 174185.
Tsu90
Tsuchiya, T. and Egami, T., "Robot Path Control with Variable Speed by
Preview Control and Adaptive Control." IF AC 11'^ Triennial World Congress,
Tallinn, Estonia, USSR, 1990, pp: 121126.
Wan93
Wang, S., Tsuchiya, T., and Hashimoto, Y., "Path Tracking Control of Robot
Manipulators Utilizing Future Information of Desired Trajectory." Trans. Of
Japan Society of Mechanical Engineers, Part C, Vol. 5, No. 564, Aug. 1993,
pp: 25122518.
Wan94
Wang, S. and Tsuchiya, T., "Fuzzy Trajectory Planning and its Application to
Robot Manipulator Path Control." Advanced Robotics, Vol. 8, No. 1, 1994,
pp: 7393.
Xia94
Xiangrong, X. and Xiangfeng, M., "A Method for Trajectory Planning of
Robot Manipulators." Modeling, Measurement & Control B:, ASME Press,
Vol.54,No. 3, 1994,pp:3336.
Zad69
Zadeh, L. A., "Fuzzy Sets." Information Control, Vol. 8, No. 3, 1965, pp:
338353.
Zel95
Zelek, J. S., "Dynamic Path Planning." Proc. of the IEEE International
Conference on Systems, Man and Cybernetics, Vol. 2, 1995, Piscataway, NJ,
pp: 12851290.
BIOGRAPHICAL SKETCH
Waheed A. Abbasi was bom on August 12, 1968 in the southern portcity of
Karachi, Pakistan. To undertake his undergraduate studies, he moved to the Comhusker
state, where he received his Bachelor of Science degree in Mechanical Engineering from
the University of NebraskaLincoln (UNL) in 1991. His interest in Robotics and his
desire to pursue graduate study brought him to Gator country, where he joined the Center
for Intelligent Machines and Robotics (CIMAR), Department of Mechanical Engineering,
University of Florida (UF). He received his Master of Science degree in Mechanical
Engineering with a minor in Industrial Engineering in December 1994. Afterwards, he
continued his studies and work at CIMAR, being involved in a variety of projects as well
as conducting research. Upon completion of his doctoral degree, he intends to work in
the industry. Waheed is married to Uzma and they have a son, Bassil.
159
I certify that I have read this study and that in my opinion it conforms to
acceptable standards of scholarly presentation and is fliUy adequate, in scope and quality,
as a dissertation for the degree of Doctor of Philosophy.
Carl D. Crane III, Chairman
Professor of Mechanical Engineering
I certify that I have read this study and that in my opinion it conforms to
acceptable standards of scholarly presentation and is fully adequate, in scope and quality,
as a dissertation for the degree of Doctor of Philosophy.
Joseph ouff
Graduate Research Professor of
Mechanical Engineering
I certify that I have read this study and that in my opinion it conforms to
acceptable standards of scholarly presentation and is fully adequate, in scope and quality,
as a dissertation for the degree of Doctor of Philosophy.
yM.^^>
C. Ziegert
essor of Mechanica^ngineering
I certify that I have read this study and that in my opinion it conforms to
acceptable standards of scholarly presentation and is fully adequate, in scope and quality,
as a dissertation for the degree of Doctor of Philosophy.
jhn K. Schueller
Associate Professor of Mechanical
Engineering
I certify that I have read this study and that in my opinion it conforms to
acceptable standards of scholarly presentation and is fully adequate, in scope and quality,
as a dissertation for the degree of Doctor of Philosophy.
,1 '
Michael C. Nechyba
Assistant Professor of Electrical and
Computer Engineering
This dissertation was submitted to the Graduate Faculty of the College of
Engineering and to the Graduate School and was accepted as partial fulfillment of the
requirements for the degree of Doctor of Philosophy.
August 2001 . iP,..Xlww^ l< L..y^
Pramod P. Khargonekar
Dean, College of Engineering
Winfred M. Phillips
Dean, Graduate School
lO
200 1
DciTY OF FLORIDA
liiiifi: